(57)【要約】【課題】たくさんの人が存在する混雑な環境下で地図構築を行う。【解決手段】ロボットが移動する間に連続して撮影した連続画像を取得する連続画像取得部１１と、連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出部１２と、局所特徴量抽出部１２により抽出した局所特徴量について、連続する入力画像間でマッチングをとる特徴量マッチング１３と、特徴量マッチング部１３により所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出部１４と、連続画像を取得した各時刻におけるロボットの位置に基づき各不変特徴量に対応する距離情報を算出する距離情報取得部１５と、当該不変特徴量及び距離情報を有するハイブリッド地図として、局所メトリカル地図を生成する地図生成部１６とを有する。【選択図】図２
2012064131.tif 000002
【特許請求の範囲】
【請求項１】
移動体が移動する間に連続して撮影した連続画像を取得する連続画像取得工程と、
前記連続画像取得工程にて取得された連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出工程と、
前記局所特徴量抽出工程にて抽出された前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング工程と、
前記特徴量マッチング工程にて所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出工程と、
前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得工程と、
当該不変特徴量及び前記距離情報を有するハイブリッド地図として、局所メトリカル地図を生成する地図生成工程とを有する、地図生成方法。
【請求項２】
連続する複数の連続画像を取得した各時刻における前記移動体の移動に関する情報と、前記不変特徴量を有する特徴点の前記移動体からの計測方向を示す観測値とから、前記移動体の位置及び姿勢と、前記不変特徴量に対応する特徴点の位置とを求める位置情報取得工程とを有し、
前記地図生成工程では、前記移動体の位置及び姿勢、並びに特徴点の位置及びその不変特徴量を有する前記ハイブリッド地図を生成する、請求項１記載の地図生成方法。
【請求項３】
前記位置情報取得工程では、前記不変特徴量を求めた複数の連続画像を取得した各時刻における前記移動に関する情報の系列と、前記観測値の系列とから、確率的に尤もらしい前記移動体の位置及び姿勢、並びに前記特徴点の位置を算出する、請求項２記載の地図生成方法。
【請求項４】
前記位置情報取得工程は、ｘｉを前記移動体の位置及び姿勢としたとき、前記複数の連続画像間において、前記不変特徴量が各時刻において前記移動体からどの方向に見えるかを示す前記観測値ｚｉｊ、及び前記移動体がｘｉからｘｉ−１まで移動するのに用いた距離情報を示す入力ｕｉを取得し、前記観測値ｚｉｊと前記入力ｕｉとから下記式を最小化することで得られる、各時刻における前記移動体の位置及び姿勢、及び前記特徴点の位置を含む前記ハイブリッド地図を作成する、請求項２又は３記載の地図生成方法。
【数１７】
2012064131.tif 000019
を示す。
【請求項５】
前記不変特徴量と、前記移動体の位置及び姿勢とは、複数枚の連続画像から得られるものであって、
前記地図生成工程は、前記移動体の位置及び姿勢及び前記不変特徴量を算出するために使用する連続画像が各時刻において同数となるよう、過去の連続画像を削除すると共に、当該連続画像を削除することで見えなくなる前記不変特徴量を削除する過去画像削除工程を有する、請求項２乃至４のいずれか１項記載の地図生成方法。
【請求項６】
現在の場所が過去に訪れたことがある場所か否かを判定する場所判定工程と、
現在の場所が過去に訪れたことがある場所である場合に、前回の前記移動体の位置及び姿勢ｘＬｉ０と、今回の前記移動体の位置及び姿勢ｘＬｉとから、ｘＬｉ０及びｘＬｉを修正する軌跡修正工程とを有する、請求項２乃至４のいずれか１項記載の地図生成方法。
【請求項７】
前記軌跡修正工程では、下記式を最小化することで、前記ｘＬｉ０及びｘＬｉを修正する請求項６記載の地図生成方法。
【数１８】
2012064131.tif 000020
【請求項８】
移動体が移動する間に連続して撮影した連続画像を取得する連続画像取得手段と、
前記連続画像取得手段により取得した連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出手段と、
前記局所特徴量抽出手段により抽出した前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング手段と、
前記特徴量マッチング手段により所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出手段と、
前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得手段と、
当該不変特徴量及び前記距離情報を有するハイブリッド地図として、局所メトリカル地図を生成する地図生成手段とを有する、地図生成装置。
【請求項９】
連続する複数の連続画像を取得した各時刻における前記移動体の移動に関する情報と、前記不変特徴量を有する特徴点の前記移動体からの計測方向を示す観測値とから、前記移動体の位置及び姿勢と、前記不変特徴量に対応する特徴点の位置とを求める位置情報取得工程とを有し、
前記地図生成手は、前記移動体の位置及び姿勢、並びに特徴点の位置及びその不変特徴量を有する前記ハイブリッド地図を生成する、、請求項８記載の地図生成装置。
【請求項１０】
前記位置情報取得手段は、前記不変特徴量を求めた複数の連続画像を取得した各時刻における前記移動に関する情報の系列と、前記観測値の系列とから、確率的に最も尤もらしい前記移動体の位置及び姿勢、並びに前記特徴点の位置を算出する、請求項９記載の地図生成装置。
【請求項１１】
予め作成された局所メトリカル地図に従って経路計画を行い移動する移動体の移動方法であって、
移動体が移動する間に連続画像を取得する連続画像取得工程と、
前記連続画像取得工程にて取得された連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出工程と、
前記局所特徴量抽出工程にて抽出された前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング工程と、
前記特徴量マッチング工程にて所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出工程と、
前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得工程と、
算出した不変特徴量及び距離情報と、前記局所メトリカル地図上に登録されている不変特徴量及び距離情報を比較し、現在の位置が登録されているか否かを判定し、登録されている場合は、当該局所メトリカル地図の情報に基づき現在位置の修正を行う軌跡修正工程とを有し、
前記局所メトリカル地図は、前記移動体が前記連続画像を取得した各時刻における位置情報に基づき算出された前記各不変特徴量に対応する距離情報と、前記不変特徴量とを有するハイブリッド地図である、移動体の移動方法。
【請求項１２】
前記軌跡修正工程では、現在の場所が前記局所メトリカル地図上に登録されていた場合に、登録されている前記移動体の位置及び姿勢ｘＬｅａｒｎｅｄｉ０と、今回の前記移動体の位置及び姿勢ｘＬｉとから、ｘＬｅａｒｎｅｄｉ０及びｘＬｉを修正する、請求項１１記載の移動体の移動方法。
【請求項１３】
前記軌跡修正工程では、下記式を最小化することで、前記ｘＬｅａｒｎｅｄｉ０及びｘＬｉを修正する請求項１２記載の移動体の移動方法。
【数１９】
2012064131.tif 000021
【請求項１４】
現在の位置が、地図に登録されている場所とどれだけ違うかを示す相対座標を算出する相対座標算出工程を更に有し、
前記軌跡修正工程では、前記ｘＬｉとして前記相対座標を使用して、前記軌跡の修正を行う、請求項１３記載の移動体の移動方法。
【請求項１５】
予め作成された局所メトリカル地図に従って経路計画を行い移動するロボット装置であって、
移動体が移動する間に連続画像を取得する連続画像取得手段と、
前記連続画像取得手段により取得した連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出手段と、
前記局所特徴量抽出手段により抽出した前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング手段と、
前記特徴量マッチング手段により所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出手段と、
前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得手段と、
算出した不変特徴量及び距離情報と、前記局所メトリカル地図上に登録されている不変特徴量及び距離情報を比較し、現在の位置が登録されているか否かを判定し、登録されている場合は、当該局所メトリカル地図の情報に基づき現在位置の修正を行う軌跡修正手段とを有し、
前記局所メトリカル地図は、前記移動体が前記連続画像を取得した各時刻における位置情報に基づき算出された前記各不変特徴量に対応する距離情報と、前記不変特徴量とを有するハイブリッド地図である、ロボット装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の連続する連続画像から特徴量を抽出し、これに基づき地図を作成する地図生成装置及び方法、この地図に基づき移動する移動体の移動方法、並びにこの地図生成装置を搭載したロボット装置に関する。
【背景技術】
【０００２】
近年のロボット産業の発展に伴い、警備ロボットや介護ロボットなどの、自律移動ロボットの導入が期待されている。一般に、移動ロボットが自律的にナビゲーションを行うためには、障害物の位置情報などを表した地図が必要となる。未知の環境で移動ロボットが自律的に地図構築・自己位置推定を行う問題はＳＬＡＭ(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）を使用することができる。
【００１７】
【数１】
2012064131.tif 000003
を示す。
【００１８】
また、前記不変特徴量と、前記移動体の位置及び姿勢とは、複数枚の連続画像から得られるものであって、前記地図生成工程は、前記移動体の位置及び姿勢及び前記不変特徴量を算出するために使用する連続画像が各時刻において同数となるよう、過去の連続画像を削除すると共に、当該連続画像を削除することで見えなくなる前記不変特徴量を削除する過去画像削除工程を有することができる。これにより、常に計算速度を一定に保つことができる。
【００１９】
さらに、現在の場所が過去に訪れたことがある場所か否かを判定する場所判定工程と、現在の場所が過去に訪れたことがある場所である場合に、前回の前記移動体の位置及び姿勢ｘＬｉ０と、今回の前記移動体の位置及び姿勢ｘＬｉとから、ｘＬｉ０及びｘＬｉを修正する軌跡修正工程とを有することができる。これにより、より正確な地図を作成することができる。
【００２０】
さらに、前記軌跡修正工程では、下記の式を最小化することで、前記ｘＬｉ０及びｘＬｉを修正することができる。
【００２１】
【数２】
2012064131.tif 000004
ここで、ＰＬ、Ｑｙは、マハラノビス距離を算出する際の共分散、ｘＬｉは、ｉ番目に過去に訪れた場所であることが検出された位置、ｘＬｉ０は、ｘＬｉと同じ場所であると認識された場所、ＮＬは、過去に訪れた場所であることが検出された回数、Ｔは現在までの総ステップ数、ｖｔは、ｘｔ−１からみたｘｔの相対座標、
x_(t-1)?v_tは、移動体の軌跡ｘｔ−１の相対座標上でｖｔ移動した場合に到達する位置を示す。
【００２２】
本発明にかかる地図生成装置は、移動体が移動する間に連続して撮影した連続画像を取得する連続画像取得手段と、前記連続画像取得手段により取得した連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出手段と、前記局所特徴量抽出手段により抽出した前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング手段と、前記特徴量マッチング手段により所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出手段と、前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得手段と、当該不変特徴量及び前記距離情報を有するハイブリッド地図として、局所メトリカル地図を生成する地図生成手段とを有するものである。
【００２３】
本発明にかかる移動体の移動方法は、予め作成された局所メトリカル地図に従って経路計画を行い移動する移動体の移動方法であって、移動体が移動する間に連続画像を取得する連続画像取得工程と、前記連続画像取得工程にて取得された連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出工程と、前記局所特徴量抽出工程にて抽出された前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング工程と、前記特徴量マッチング工程にて所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出工程と、前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得工程と、算出した不変特徴量及び距離情報と、前記局所メトリカル地図上に登録されている不変特徴量及び距離情報を比較し、現在の位置が登録されているか否かを判定し、登録されている場合は、当該局所メトリカル地図の情報に基づき現在位置の修正を行う軌跡修正工程とを有し、前記局所メトリカル地図は、前記移動体が前記連続画像を取得した各時刻における位置情報に基づき算出された前記各不変特徴量に対応する距離情報と、前記不変特徴量とを有するハイブリッド地図とすることができる。
【００２４】
本発明においては、不変特徴量と距離情報を有するハイブリッド地図によりナビゲーションを実施するため、例えば食堂などの複雑な環境下においても、安定した特徴量が抽出されているため、極めて正確なナビゲーションを実施することができる。
【００２５】
また、前記軌跡修正工程では、現在の場所が前記局所メトリカル地図上に登録されていた場合に、登録されている前記移動体の位置及び姿勢ｘＬｅａｒｎｅｄｉ０と、今回の前記移動体の位置及び姿勢ｘＬｉとから、ｘＬｅａｒｎｅｄｉ０及びｘＬｉを修正することができ、Ｌｏｏｐ−ｃｌｏｓｉｎｇにより、正確なナビゲーションを実施することができる。
【００２６】
さらに、前記軌跡修正工程では、下記の式を最小化することで、前記ｘＬｅａｒｎｅｄｉ０及びｘＬｉを修正することができる。
【００２７】
【数３】
2012064131.tif 000005
ここで、ＰＬ、Ｑｙは、マハラノビス距離を算出する際の共分散、ｘＬｉは、ｉ番目に地図に登録されている場所であることが検出された位置、ｘＬｅａｒｎｅｄｉ０は、ｘＬｉと同じ場所であると認識された場所、ＮＬは、地図に登録された場所であることが検出された回数、Ｔは現在までの総ステップ数、ｖｔは、ｘｔ−１からみたｘｔの相対座標、
x_(t-1)?v_tは、移動体の軌跡ｘｔ−１の相対座標上でｖｔ移動した場合に到達する位置を示す。
【００２８】
さらにまた、現在の位置が、地図に登録されている場所とどれだけ違うかを示す相対座標を算出する相対座標算出工程を更に有し、前記軌跡修正工程では、前記ｘＬｉとして前記相対座標を使用して、前記軌跡の修正を行うことができる。現在の位置と登録位置との相対座標を求めてＬｏｏｐ−Ｃｌｏｓｉｎｇすることにより、更に正確なナビゲーションを実施することができる。
【００２９】
本発明にかかるロボット装置は、予め作成された局所メトリカル地図に従って経路計画を行い移動するロボット装置であって、移動体が移動する間に連続画像を取得する連続画像取得手段と、前記連続画像取得手段により取得した連続画像から各特徴点における局所特徴量を抽出する局所特徴量抽出手段と、前記局所特徴量抽出手段により抽出した前記局所特徴量について、前記連続する入力画像間でマッチングをとる特徴量マッチング手段と、前記特徴量マッチング手段により所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める不変特徴量算出手段と、前記連続画像を取得した各時刻における前記移動体の位置に基づき前記各不変特徴量に対応する距離情報を算出する距離情報取得手段と、算出した不変特徴量及び距離情報と、前記局所メトリカル地図上に登録されている不変特徴量及び距離情報を比較し、現在の位置が登録されているか否かを判定し、登録されている場合は、当該局所メトリカル地図の情報に基づき現在位置の修正を行う軌跡修正手段とを有し、前記局所メトリカル地図は、前記移動体が前記連続画像を取得した各時刻における位置情報に基づき算出された前記各不変特徴量に対応する距離情報と、前記不変特徴量とを有するハイブリッド地図とすることができる。
【発明の効果】
【００３０】
本発明によれば、たくさんの人が存在する混雑な環境下で地図構築を行い、人間が教えた道順通りに自律的に移動するロボット装置、移動体の移動方法、当該ロボット装置が使用する地図を生成する地図生成装置及び地図生成方法を提供することができる。
【図面の簡単な説明】
【００３１】
【図１】本発明の実施の形態１にかかるハイブリッド地図を示す図である。
【図２】本発明の実施の形態１にかかる地図生成装置を示す図である。
【図３】本発明の実施の形態１にかかる特徴量ＰＩＲＦを抽出する方法を説明する図である。
【図４】本発明の実施の形態１におけるＰＩＲＦ及び３Ｄ−ＰＩＲＦの概念を示す図である。
【図５】本発明の実施の形態１におけるＳＷＦを示す図であって、３Ｄ−ＰＩＲＦを算出方法を説明する図である。
【図６】本発明の実施の形態１にかかる地図生成方法を示すフローチャートです。
【図７】本発明の実施の形態２にかかる本実施の形態にかかるナビゲーション装置を示すブロック図である。
【図８】本発明の実施の形態２における相対座標の算出方法を説明する図である。
【図９】本発明の実施の形態２にかかるロボットのナビゲーション方法を示す図である。
【図１０】本発明の実施の形態２にかかるロボットを示すブロック図である。
【図１１】本発明の実施例の屋内環境を示す図である。
【図１２】本発明の実施例の環境の真値地図と、学習フェーズでロボットを移動させた道順を示す図である。
【図１３】本発明の実施例において、カメラの情報を使わずオドメトリのみから計算したロボットの軌跡を示す図である。
【図１４】本実施の形態の方法により、ロボットが学習した軌跡を示す図である。
【図１５】ロボットが自律的に移動した軌跡を示す図である。
【発明を実施するための形態】
【００３２】
以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。この実施の形態は、本発明を、局所地図を用いたハイブリッド地図（局所メトリカル地図）を生成する地図生成装置、及びこの局所メトリカル地図を利用してナビゲーションするロボット装置に適用したものである。
【００３３】
本発明においては、たくさんの人が存在する混雑な環境下でオンライン・追加的に地図構築・自己位置推定を行い、人間が教えた道順通りに自律的に移動することを目的としている。
【００３４】
混雑な環境下でも安定な３次元上の特徴点の抽出画像を用いたＳＬＡＭにおいて多く用いられている特徴点として、ＳＩＦＴやコーナー点がある。一般にＳＩＦＴやコーナー点などの局所特徴点は対応付けが取りやすく、画像を用いたＳＬＡＭに適している。しかし、多くの人が入り乱れる混雑な環境下でロボットが取得した画像からＳＩＦＴを抽出すると、ロボットが少し移動しただけで消えてしまうような弱い特徴点や、動いている人物からの特徴点が多く抽出されてしまうという問題があった。そのような不安定な特徴点が多いと、無駄に処理時間が延びるだけでなく、地図構築に悪影響を及ぼすという問題が起こる。
【００３５】
これに対し、本件出願人は、特願２００９−２００７６４、特願２０１０−０８１０２７、及び特願２０１０−１１５３０７において、ＰＩＲＦ（Position Invariant Robust Feature）という特徴量を提案している。ＰＩＲＦは、後述するように、複数枚の連続画像からＳＩＦＴを抽出して連続的にマッチングを取り、全ての画像でマッチングが取れた特徴点のみを抽出する手法である。
【００３６】
ＰＩＲＦはロボットの移動に対して見た目の変化の小さい部分を特徴として抽出することができ、それにともなって、動いている人物などから抽出される見た目の変化の大きい部分の特徴点を除外できるという特徴がある。
【００３７】
ところで、ＳＬＡＭには大きく分けて、地図を連続的な空間配置で表すメトリカルな手法と、地図が離散的なグラフ構造で表されるトポロジカルな手法と、それらを組み合わせたハイブリッドな手法がある。ハイブリッドな手法では、各ノードが局所的なメトリカル地図の情報を持つようなトポロジカルな地図が構築される。一方、ハイブリッドな手法はグラフ構造を有するため経路計画が容易であり、距離情報も用いることが可能であるため、最短経路の探索など、より高度な経路計画を行うことが可能であるという特徴がある。
【００３８】
そこで、本願発明者等は鋭意実験研究し、上記ＰＩＲＦを利用して複数の画像データから特徴点を取り出すことで、たくさんの人が存在する混雑な環境下でも安定した特徴点を抽出し、これを３次元空間にマッピングすることで、ＰＩＲＦを利用したハイブリッド地図を構築する方法を見出した。
【００３９】
本来ＰＩＲＦは、画像から抽出される画像上の特徴点であり、直接ＳＬＡＭに適用することはできない。さらに、ＰＩＲＦは、１枚の画像からではなく、複数枚の連続する連続画像から抽出される特徴量である。したがって、単純には、この特徴量に距離情報を結びつけることができない。そこで、本願発明者等は、鋭意実験研究し、連続画像のＳＩＦＴにオドメトリの情報を組み合わせることで、ＰＩＲＦを３次元空間にマッピングする（３Ｄ−ＰＩＲＦ）手法を見出した。
【００４０】
本発明の実施の形態１．
図１は、本実施の形態にかかるハイブリッド地図を示す図である。ロボットの軌跡はノードｘｔとエッジｖｔによるグラフで表され、各ノードは３Ｄ−ＰＩＲＦの位置ｍｘと分散Πｘの情報を有し、各エッジはノード間の相対位置ｖの情報を有する。なお、ｖｔは、ｘｔ−１から見たｘｔの相対座標として計算することができる。
【００４１】
まず、本実施の形態にかかる地図生成装置について説明する。図２は、本実施の形態にかかる地図生成装置を示す図である。図２に示すように、本実施の形態にかかる地図生成装置１は、連続画像取得部１１、局所特徴量抽出部１２、特徴量マッチング部１３、不変特徴量算出部１４、距離情報取得部１５、及び地図生成部１６を有する。
【００４２】
連続画像取得部１１は、ロボット等の移動体が移動する間に連続して撮影した連続画像を取得する。局所特徴量抽出部１２は、連続画像取得部１１により取得した連続画像から各特徴点における局所特徴量を抽出する。特徴量マッチング部１３は、局所特徴量抽出部１２により抽出した局所特徴量について、連続する入力画像間でマッチングをとる。不変特徴量算出部１４は、特徴量マッチング部１３により所定数連続する画像間でマッチングが取れた局所特徴量の平均を不変特徴量として求める。距離情報取得部１５は、連続画像を取得した各時刻におけるロボットの位置（座標等）に基づき各不変特徴量に対応する距離情報を算出する。地図生成部１６は、この不変特徴量及び距離情報を有するハイブリッド地図として、局所メトリカル地図を生成する。
【００４３】
ここで、距離情報取得部１５又は地図生成部１６は、連続する複数の連続画像を取得した各時刻におけるロボットの移動に関する情報と、不変特徴量（ＰＩＲＦ）を有する特徴点のロボットからの計測方向を示す観測値とから、ロボットの位置及び姿勢と、ＰＩＲＦに対応する特徴点の位置とを求める位置情報取得部（不図示）を有している。これにより、地図生成部１６は、ロボットの位置及び姿勢、並びに特徴点の位置及びそのＰＩＲＦを有するハイブリッド地図を生成することができる。
【００４４】
また、位置情報取得部は、ＰＩＲＦを求めた複数の連続画像を取得した各時刻におけるロボットに関する情報の系列と、観測値の系列とから、確率的に最も尤もらしいロボットの位置及び姿勢、並びに特徴点の位置を算出する。このロボットの位置及び姿勢、特徴点の位置の求め方についての詳細は後述する。
【００４５】
以下、本実施の形態にかかる地図生成装置１について、詳細い説明する。先ず最初に、不変特徴量であるＰＩＲＦの算出方法について説明する。上述したように、本件出願人は、先に特願２００９−２００７６４、特願２０１０−０８１０２７、及び特願２０１０−１１５３０７により、ＰＩＲＦの算出方法について開示している。
【００４６】
ＰＩＲＦは、近くの対象については撮影位置や撮影時間帯の変化による見え方の差（特徴量変化）が大きいが、遠くの対象については変化が小さい（ランドマークの特徴量はあまり変化しない）ことに着目し、混雑下でも誤認識しにくい特徴量として考えられたものである。
【００４７】
簡単には、連続画像間で局所特徴のマッチングを行い、予め定めた枚数間で、連続してマッチングのとれている特徴を選択し、選択された特徴において、それとマッチングのとれている全特徴の平均を特徴量ＰＩＲＦとして抽出・記述するものである。
【００４８】
先ず、連続画像取得部１１は、連続画像を取得する。ＰＩＲＦは、複数枚の連続画像から抽出される特徴量であるため、入力としては、連続する画像、すなわちビデオ画像が必要となり、例えば全方位画像を使用することができる。ＰＩＲＦで要求される連続画像とは、ある画像セットであって、一定のフレームで、例えば１秒毎に２フレームなど、毎秒毎に連続的に撮影されたビデオ画像をいう。すなわち、ビデオからキャプチャされた画像は一般的に連続的であり、ＰＩＲＦにおける連続画像は、ビデオ画像を使用したものでなければならない。画像の取得率は、カメラの速度に応じて設定される。たとえば、カメラが車に搭載されていた場合、カメラの速度は１分間に約１０００ｍ／分であり、ビデオからキャプチャされる連続画像はおよそ５０乃至１００フレーム／秒となる。
【００４９】
図３は、本実施の形態にかかる特徴量ＰＩＲＦを抽出する方法を説明する図である。局所特徴量抽出部１２は、既存の局所特徴量抽出方法を使用して局所特徴量を抽出する。局所特徴量抽出部１２は、例えば、ＳＩＦＴ（Scale Invariant Feature Transformation）、又はＳＵＲＦ（Speed Up Robustness Features）の特徴量を使用することができる。または、これらＳＩＦＴやＳＵＲＦに限らず、他の局所特徴量を使用することができることは勿論である。特に、スケール、回転の変動、又はノイズ等に対してロバストな他の局所特徴量を用いることが好ましい。これらの局所特徴量を用いることで、既存の特徴量が有する性能もそのまま引き継がれ、照明変化等にも頑健な特徴として抽出・記述することが可能となる。ここで、ｉ番目のエリアにおける全方位画像の数をｎｉとし、その中でｊ番目の画像から抽出された局所特徴量の集合をＵｊｉとする。ここでは、局所特徴量はｕ→で表す。
【００５０】
次に、特徴量マッチング部１３が、連続する２枚の画像間全てについて局所特徴量ｕ→を構成する各特徴量のマッチングを行う。すなわち、ｊ＝ｑ番目の画像の全局所特徴量について、ｊ＝ｑ＋１番目の画像の全局所特徴量に対してマッチングを行う。ここでは、それぞれマッチングのとれた特徴へのインデックスをマッチング結果ベクトルｍｑｉ→として求める。
【００５１】
ここで、マッチング方法の一例について、ＳＩＦＴを例にとって説明する。画像Ｉａから抽出される特徴をｖとする。この特徴ｖが次の画像Ｉａ＋１の特徴ｖ'とマッチングするか否かを判定する。先ず、特徴ｖと画像Ｉａ＋１から抽出した全ての特徴との間のドット積（dot product）を求める。そして、最も類似する特徴ｖｆｉｒｓｔと、２番目に類似する特徴ｖｓｅｃｏｎｄを求めるために、この結果をソートする。もし、
（ｖｆｉｒｓｔ・ｖ）／（ｖｓｅｃｏｎｄ・ｖ）＞θ
が成立する場合、マッチングのとれた特徴ｖ'＝ｖｆｉｒｓｔと判定する。ここで、閾値θはたとえば０．６とすることができる。上記が成立しない場合は、画像Ｉａにおける特徴ｖにマッチングする特徴は、画像Ｉａ＋１には存在しないと判定する。
【００５２】
図３に示すように、各入力画像から６つの局所特徴量が抽出された場合について説明する。これら６つの局所特徴量間でマッチングを取り、マッチングが取れた場合にのみ、マッチングが取れた特徴量へのインデックスを付す。例えば、ｍ１ｉ→の１番目の局所特徴量は、ｍ２ｉ→の３番目の局所特徴量とマッチングがとれていることを示し、ｍ２ｉ→の３番目の特徴量は、ｍ３ｉ→の６番目の特徴量とマッチングが取れていることを示す。
【００５３】
次に、連続特徴量選択部（不図示）は、連続特徴量を選択する。先ず、いくつのｍｑｉ→を使用して連続特徴量を求めるかを決定する。このｍｑｉ→の数を、本明細書においては、ウィンドウサイズｗともいう。また、このウィンドウサイズｗに含まれるｍｑｉ→の集合を、サブプレイスという。ここで、ウィンドウサイズｗが大きいほどより頑健な、識別力の高い連続特徴量のみを抽出できるが、大きすぎると特徴数が極端に少なくなってしまう。また、小さすぎると、頑健ではない、識別力のないような特徴量も抽出してしまうので、目的等に応じて最適な大きさとする必要がある。
【００５４】
本実施の形態においては、ウィンドウサイズｗを３とする。したがって、連続する４枚の入力画像を使用して連続特徴量を求める。すなわち、図２に示すように、１番目のサブプレイスには、ｍ１ｉ→、ｍ２ｉ→、ｍ３ｉ→が含まれ、入力画像Ｉ１、Ｉ２、Ｉ３、Ｉ４が対応する。なお、インデックスの数が０の場合は、次にマッチングする特徴量がないことを示す。よって、図２の場合、１番目のサブプレイスには、３つの連続特徴量が含まれることになる。
【００５５】
連続特徴量選択部２３は、ウィンドウサイズｗを設定したらこのウィンドウｗをひとつずつずらしながら、そのウィンドウサイズに含まれる全方位画像４枚内で共通して出現する特徴を連続特徴量として抽出する。ウィンドウサイズｗを設定したら、連続特徴量を抽出するために用いる関数を以下のように定義する。ただし、ｂは注目するインデックスベクトルの番号とする。
【数４】
2012064131.tif 000006
そして、全てのマッチング結果ベクトルｍｑｉ→について、ｆ（ｍｘ,ｙｉ）を計算し、ｆ（ｍｘ,ｙｉ）＞０となるときの局所特徴量ｕｘ,ｙｉ→のみを抽出する。入力画像の数がｎｉ、ウィンドウサイズがｗのとき、サブプレイスの数は、ｎｉ−ｗ＋１となる。なお、このウィンドウサイズは、可変であってもよく、周りの状況等に合わせて、適宜変更してもよい。
【００５６】
不変特徴量算出部１４は、同一ウィンドウグループであるサブプレイス内においてマッチングがとれた局所特徴量の平均を求める。これらの平均値からなるベクトルによりＰＩＦＲ辞書が構成される。全サブプレイス（ｎｉ−ｗ＋１）個から抽出された（ｎｉ−ｗ＋１）個のサブプレイスＰＩＲＦ辞書（Ｄｊｉ，ｊ≦ｎｉ−ｗ＋１）をＰＩＲＦ辞書（Ｄｉ）に登録する。このＰＩＲＦ辞書を構成する、マッチングがとれた局所特徴量の平均各がＰＩＲＦである。
【００５７】
このように抽出されたＰＩＲＦは各画像から抽出されたＳＩＦＴ特徴点のうち、すべての隣り合った画像間でマッチングがとれた特徴点として抽出されるため、見た目の変化の大きい部分の特徴点を除外することができる。
【００５８】
次に、このＰＩＲＦを使用して局所メトリカル地図を生成する方法について説明する。図１に戻って、先ず、連続画像とオドメトリから３Ｄ−ＰＩＲＦを算出する（Ｓ１０１）。ここで、３Ｄ−ＰＩＲＦを抽出するためには、特徴点の３次元位置を計算する必要があるが、３Ｄ−ＰＩＲＦは、過去数ステップの観測データを用いるため、ロボット等の移動体の現在の状態しか保持しないカルマンフィルタなどの手法と組み合わせることができない。
【００５９】
そこで、本実施の形態においては、ＳＷＦ（Sliding Window Filter）を用いる。ＳＷＦは、一定期間よりも古い姿勢・特徴点を消去していくことで、計算時間を一定に保つことができるという特徴がある。これを用いることで、３Ｄ−ＰＩＲＦの３次元位置を計算することができ、連続画像とオドメトリの情報から３Ｄ−ＰＩＲＦを抽出することが可能となる。この３Ｄ−ＰＩＲＦは複数のステップにおいて、取得された連続画像と、各ステップ間のオドメトリの情報から、３次元上の特徴点として抽出される。なお、本実施の形態においては、ＰＩＲＦと位置情報を結びつける手段としてＳＷＦの手法を用いるが、ＰＩＲＦを用いたハイブリッド地図を作成できればよく、その他の方法を用いてもよい。
【００６０】
図４は、ＰＩＲＦ及び３Ｄ−ＰＩＲＦの概念を示す図である。各時刻における連続画像のＳＩＦＴからＰＩＲＦが算出されると共に、各時刻におけるロボットなどの移動体の位置（座標）及び姿勢（角度）、オドメトリ、各時刻における移動体からみた特徴点に観測値（移動体に対する特徴点の角度）から、３Ｄ−ＰＩＲＦが算出される。
【００６１】
図５は、ＳＷＦを示す図であって、３Ｄ−ＰＩＲＦを算出方法を説明する図である。ベクトルｘ＝[ｘ０Ｔ，・・・，ｘＭＴ]Ｔは、各時刻での移動体（ロボット）の位置及び姿勢を示す。ｚｉｊは、ロボットの姿勢ｊから観測した特徴点ｉの観測値を示す。観測値とは、特徴点のロボットからの計測方向を示し、ロボットに対する特徴点の角度として表わされる。また、ｕｉは、ロボットがある位置及び姿勢ｘｉ−１から次の位置及び姿勢（次の時刻の位置及び姿勢）ｘｉまで移動するのに用いた入力を示す。この入力とは、例えば、車輪型のロボットであれば、車輪に与えられたスピード及び回転数を示し、これらの値により、距離を算出することができる。
【００６２】
ＳＷＦは、入力の系列ｕ＝[ｕ１，・・・，ｕｍ−１]Ｔと、観測値ｚ＝[ｚ１１，・・・，ｚｎｍ]Ｔから、事後確率ｐ（ｘ，ｍ│ｕ，ｚ）を最大化する
【００６３】
【数５】
2012064131.tif 000007
【００６４】
まず、ｘ、ｍ、ｕ、ｚの同時確率は、以下のようになる。
【００６５】
【数６】
2012064131.tif 000008
ここで、ｐ（ｘ０）は、ロボットの位置と姿勢に関する事前分布、ｐ（ｍ）は、地図に関する事前分布、ｐ（ｘｔ│ｘｔ−１，ｕｔ）は、姿勢ｘｔ−１で入力ｕｔが加わった時のｘｔの動作モデル、ｐ（ｚｉ│ｘｔ，ｍｉ）は、ロボットの位置ｘｔから特徴点ｍｉを見たときの観測値ｚｉの計測モデルである。
【００６６】
これらを、それぞれ以下のようなガウス分布と仮定する。
【００６７】
【数７】
2012064131.tif 000009
【００６８】
【数８】
2012064131.tif 000010
【００６９】
【数９】
2012064131.tif 000011
【００７０】
【数１０】
2012064131.tif 000012
ｆは動作モデル、ｈは観測モデルを示す。
【００７１】
以上の設定の下、以下の（８）式を最小化することにより、最大事後確率となるｘ＾、ｍ＾を求めることができる。
【００７２】
【数１１】
2012064131.tif 000013
この（８）式を最適化することにより、ｘ＾、ｍ＾と、それに対応する共分散（共分散行列Π＾）を求めることができる。
【００７３】
次に、これらの値を用いて、ロボットの状態と各特徴点の相対的な位置関係（局所地図）が登録されたハイブリッド地図１７を構築する。
【００７４】
先ず、特徴点の位置ｍ＾と、その共分散行列Π＾を、局所地図として保存する。特徴点の位置ｍ＾及び共分散行列Π＾を各ステップで保存していくことで、ハイブリッド地図のノード（局所地図）を構築することができる。
【００７５】
また、ハイブリッド地図のエッジが有する情報である、ノード間の相対位置を求める。この相対位置は、まず、（８）式を最小化することで、上記のようにロボットの位置・姿勢ｘ＾を求めることができる。それによって、各時刻でのロボットの位置及び姿勢ｘｔが求められる。これにより、下記式（９）
【００７６】
【数１２】
2012064131.tif 000014
によってｘｔ−１から見たｘｔの相対座標ｖｔを求めることができる。式（９）は、ｘｔ−１から見たｘｔの相対座標を示している。この相対座標ｖｔをエッジの情報として保存する。以上の処理により、ハイブリッド地図を構築することができる。
【００７７】
次に、本実施の形態においては、ロボットの位置及び姿勢及びＰＩＲＦを算出するために使用する連続画像が各時刻において同数となるよう、過去の連続画像を削除すると共に、当該連続画像を削除することで見えなくなるＰＩＲＦを削除する。
【００７８】
なお、ロボットの位置及び姿勢及びＰＩＲＦは、複数の連続画像から算出するが、この複数の連続画像の枚数は可変であってもよい。例えば、特徴量が特に多い場所では、連続画像の枚数を増やしたり、逆に、特徴量が少ない場所では、連続画像の枚数を減らしたりすることも可能である。
【００７９】
ここで、ある時刻でロボットの位置及び姿勢及びＰＩＲＦを算出し、次の時刻でロボットの位置及び姿勢及びＰＩＲＦを算出する際に、もっとも過去の連続画像を破棄することができる。これにより、メモリ容量を減らし、演算速度を一定に保つことができる。
【００８０】
これは、ＳＷＦにおいては、marginalizing outと呼ばれる手法である。marginalizing out は、過去のロボットの姿勢、その姿勢から見えていた特徴点を周辺化によって削除する処理であり、この処理によって、計算で扱われるロボットの姿勢数Ｍが常に一定となり、各ステップの計算時間を一定に保つことができる。
【００８１】
以上のように、過去数ステップ分の観測値を用いることができるＳＷＦを用いることで、特徴量としてＰＩＲＦを用いた地図を構築することが可能となる。なお、本実施の形態においては、ロボットは各ステップごとに、特徴点の位置ｍ＾をロボットから見た相対座標上に座標変換したものと、その共分散を、局所地図として保存することで、各ノードが局所地図を表すハイブリッド地図を構築する。このように３次元上の特徴点である３Ｄ−ＰＩＲＦを用いることで、後述するように、人の多い混雑な環境下でもロバストに、かつ特徴点の位置情報を用いたＳＬＡＭナビゲーションを行うことが可能となる。
【００８２】
次に、本実施の形態にかかる地図生成部１６は、現在の場所が過去に訪れたことがある場所か否かを判定し、現在の場所が過去に訪れたことがある場所である場合に、前回の移動体の位置及び姿勢ｘＬｉ０と、今回の移動体の位置及び姿勢ｘＬｉとから、ｘＬｉ０及びｘＬｉを修正する。これを、Ｌｏｏｐ−Ｃｌｏｓｉｎｇという（図１，Ｓ１０２）。
【００８３】
一般的にＳＬＡＭにおいては、移動距離が長くなるほど誤差が蓄積していくという問題がある。したがって、これを修正する必要がある。地図の誤差修正を行うためには、現在いる場所が過去に訪れたことのある場所かどうかを認識する必要がある。
【００８４】
過去に一度訪れたことがある場所であれば、局所メトリカル地図が構築されている。局所メトリカル地図には、その位置におけるＰＩＲＦが保存されている。ロボット装置は、連続画像を取り込む各タイミング（ノード）において、ＰＩＲＦを算出し、保存されている各ノードのＰＩＲＦと一致するか否かを判定する。例えば、９０％以上の特徴量が一致した場合、その位置は、過去に訪れたことがある位置であると判定することができる。なお、判定基準は、９０％に限らず、９０％以下でも以上であってもよい。
【００８５】
そして、過去に訪れた場所であると判定された場合は、下記数（１０）を最小化することで、ｘＬｉ０及びｘＬｉを修正する。
【００８６】
【数１３】
2012064131.tif 000015
ここで、ＰＬ、Ｑｙは、マハラノビス距離を算出する際の共分散、ｘＬｉは、ｉ番目に過去に訪れた場所であることが検出された位置、ｘＬｉ０は、ｘＬｉと同じ場所であると認識された場所、ＮＬは、過去に訪れた場所であることが検出された回数、Ｔは現在までの総ステップ数、ｖｔは、ｘｔ−１からみたｘｔの相対座標、
【００８７】
【数１４】
2012064131.tif 000016
は、移動体の軌跡ｘｔ−１の相対座標上でｖｔ移動した場合に到達する位置を示す。この（１０）式を最小化することにより、軌跡ｘ＝[ｘ０Ｔ，・・・，ｘＴＴ]Ｔの修正を行うことができる。また、Ｌｏｏｐ−Ｃｌｏｓｉｎｇが検出された位置では、地図の事前分布として、前に訪れた際に構築した局所地図の特徴点の位置を（８）式のｍの平均に代入し、共分散をΠに代入することで、過去の局所マップを考慮した地図構築を行うことができる。このように過去の局所地図と現在の観測データを組み合わせることで、局所地図の精度の向上を行うことが可能となる。
【００８８】
次に、本実施の形態にかかる地図生成方法について説明する。図６は、本実施の形態の地図生成方法を示すフローチャートです。地図は、例えば、連続画像を取得できる移動体が移動しながら連続画像を取得しつつ、順次構築することができる。ここでは、ロボットが連続画像を取得しながら、地図を構築する例について説明する。当該ロボットには、図２に示す地図生成装置が搭載されている。なお、ロボットの構成例については、後述する。
【００８９】
図６に示すように、先ず、連続画像を取得する(ステップＳ１)。次に、局所特徴量抽出部１２は、複数の連続画像のＳＩＦＴを計算する(ステップＳ２)。特徴量マッチング部１３が各連続画像間でＳＩＦＴのマッチングを取り、不変特徴量算出部１４が、マッチングが取れたＳＩＦＴの平均値をＰＩＲＦとして算出する。
【００９０】
これと並行して、距離情報取得部１５又は地図生成部１６の有する位置情報取得部は、上記（８）式を最小化し、最大事後確率ｘ＾、ｍ＾及びそれに対応する共分散を算出する(ステップＳ４)。
【００９１】
ここで、最大事後確率算出の際には、例えば、ロボットの位置ｘ０、ｘ１、ｘ２の地点で撮影された３つの連続画像を使用するとする。この場合、次の最大事後確率及びＰＩＲＦの算出には、ロボットの位置ｘ１、ｘ２、ｘ３の地点で撮影された３つの連続画像を使用し、地点ｘ０の連続画像は使用しない。そして、このｘ０の地点の連続画像が含まれないことで、認識されなくなったＰＩＲＦも削除される。いいかえれば、過去のロボットの姿勢、その姿勢から見えていた特徴点を周辺化によって削除する(ステップＳ４)。これにより、計算で扱われるロボットの姿勢数Ｍが常に一定となり、各ステップの計算時間を一定に保つ。
【００９２】
地図生成部１６は、ステップＳ３で得られた最大事後確率ｘ＾、ｍ＾及びそれに対応する共分散を用いて、ロボットの状態と各特徴点の相対的な位置関係(局所地図)を構築する(ステップＳ５)。
【００９３】
ここで、ロボットは、常に、現在の場所が過去に訪れたことがある場所か否かの判断を行っている(ステップＳ６)。ロボットは、連続画像を取得した各地点におけるＰＩＲＦの情報を有している。したがって、ロボットは移動しながらＰＩＲＦを算出するが、算出したＰＩＲＦが、保有しているＰＩＲＦと一致するか否かの判定を行っている。例えば９０％以上のＰＩＲＦが一致した場所を、過去に訪れたことがある場所と判定する。
【００９４】
過去に訪れたことがある場所と判定された場合は、上記（９）式を最小化することで、軌跡ｘの修正を行う(ステップＳ７)。移動距離が長くなると、誤差が蓄積してしまうが、このステップＳ７（Ｌｏｏｐ−ｃｌｏｓｉｎｇ）を実施することで、蓄積する誤差をリセットすることができる。そして、以上のステップＳ１乃至Ｓ７の処理を繰り返すことで、所望の地図を生成することができる。なお、ステップＳ７を実行するために、地図を作成する場合は、同一の経路を２回通る必要がある。これにより、より精度が高い地図を生成することができる。ただし、１回通ったのみで精度が高い地図が得られる場合は、ステップＳ７の処理を省略することも可能である。
【００９５】
本発明の実施の形態２．
次に、実施の形態２について説明する。本実施の形態においては、動いている物体を取り除いて処理する方法として、画像から混雑な環境下でも安定な３次元上の特徴点を抽出し、その特徴点を用いることで、ロバストにＳＬＡＭ・ナビゲーションを行う。その一例として本実施の形態においては、ロボットが、実施の形態１で作成した局所メトリカル地図を用いて、ナビゲーションする方法について説明する。
【００９６】
図７は、本実施の形態にかかるナビゲーション装置を示すブロック図である。図７に示すように、ナビゲーション装置は、連続画像取得部３１、局所特徴量抽出部３２、特徴量マッチング部３３、不変特徴量算出部３４、位置情報取得部３５、地図生成部３６、及び相対座標算出部３７を有し、局所メトリカル地図格納部４０に接続され、局所メトリカル地図が参照可能に構成されている。ここで、相対座標算出部３７以外の構成は、実施の形態１と同様であり、その動作も同様である。
【００９７】
このナビゲーション装置３０は、実施の形態１同様、ロボットに搭載される。ロボットは、始点と終点が与えられると、局所メトリカル地図格納部４０の局所メトリカル地図を参照し、経路計画を立てる。そして、その経路計画にしたがって、始点から終点まで移動する。その際、移動しながら、上述のステップＳ１乃至ステップＳ６までの処理を実施し、ナビゲーションフェーズにおいても、ハイブリッド地図１７を生成する。
【００９８】
なお、ステップＳ７までの処理を繰り返し、軌道を修正しながら、終点まで移動することも可能である。ここで、本実施の形態にかかるナビゲーション装置３０は、相対座標算出部３７を有している。この相対座標算出部３７は、ステップＳ６を実施し、現在の位置が過去に訪れた位置であると判定した場合、現在の位置が、過去における位置とどれだけずれているかを示す相対座標を算出する。
【００９９】
図８は、相対座標の算出方法を説明する図である。図８において、局所メトリカル地図を生成する際に通った位置をｘ０、ｘ１、ｘ２とする。現在の位置をｘ１'とする。この場合、現在の位置ｘ１'から見た特徴点ｍ１、ｍ２の位置は、（８）式を最小化することで得られる。局所メトリカル地図には、位置ｘ１からの特徴点ｍ１、ｍ２の位置の情報を含む。したがって、現在の位置ｘ１'が、過去の位置ｘ１からどれだけずれているかを算出することができる。
【０１００】
例えば、現在の位置ｘｔ'＝ｘｔ＋αとすると、相対位置αは、観測データから相対座標を計算できる任意のアルゴリズムを用いることで計算することができる。この任意のアルゴリズムとしては、５点アルゴリズム、８点アルゴリズム等がある（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)）。
【０１０１】
例えば、８点アルゴリズムは、図８において、ｘｔとｘｔ'の観測データ（特徴点の位置）を比較し、相対姿勢を求めるための手法である。８点アルゴリズムによって相対座標αが計算された場合には、以下の（１１）式を最小化することによって軌跡を修正することができる。
【０１０２】
【数１５】
2012064131.tif 000017
ここで、αｉは８点アルゴリズムによって計算されたｘＬｉと、ｘＬｅａｒｎｅｄｉ０との間の相対姿勢を示す。また、ＰＬ、Ｑｙは、マハラノビス距離を算出する際の共分散、ｘＬｉは、ｉ番目に地図に登録されている場所であることが検出された位置、ｘＬｅａｒｎｅｄｉ０は、ｘＬｉと同じ場所であると認識された場所であり、固定値である。ＮＬは、地図に登録された場所であることが検出された回数、Ｔは現在までの総ステップ数、ｖｔは、ｘｔ−１からみたｘｔの相対座標、
【０１０３】
【数１６】
2012064131.tif 000018
は、移動体の軌跡ｘｔ−１の相対座標上でｖｔ移動した場合に到達する位置を示す。
【０１０４】
このように、本実施の形態においては、単に（１１）式を最小化して軌跡の修正を行うのではなく、現在の位置ｘ１'と、局所メトリカル地図に登録されている過去の位置ｘ１との相対座標を算出し、これを使用して、（１１）式を最小化することにより、より精度よく、ナビゲーションを行うことができる。
【０１０５】
図９は、本実施の形態にかかるロボットのナビゲーション方法を示す図である。図９において、ステップＳ１１乃至Ｓ１６は、実施の形態１における図６のステップＳ１乃至Ｓ６と同様である。すなわち、実施の形態１と同様、３Ｄ−ＰＩＲＦを算出し、現在の位置が過去に訪れた位置であるかの判定を行う(ステップＳ１６)。そして、現在の位置が過去に訪れた位置である場合は、現在の位置と過去の位置の相対座標を算出する(ステップＳ１７)。そして、この相対座標を、（１１）式に代入し、これを最小化することで、軌跡ｘの修正を行う(ステップＳ１８)。その後は、ステップＳ１１からの処理を繰り返す。
【０１０６】
ナビゲーションフェーズでは、学習フェーズで教えられた道順通りにロボットが自律的に移動する。移動ロボットは学習フェーズと同様にハイブリッド地図を構築しながら移動するが、ナビゲーションフェーズでは、教えられた道順通りに移動するために、学習フェーズで学習したハイブリッド地図と現在のハイブリッド地図それぞれの軌跡と局所マップを比較し、学習した軌跡の方向にロボットの位置を修正することで、学習した軌跡に追従するように行動計画を行う。
【０１０７】
このようなナビゲーションを行うロボットの一例について説明しておく。図１０は、本実施の形態にかかるロボットを示すブロック図である。ロボット５０は、制御部５１、入出力部５２、駆動部５３、電源部５４、及び外部記憶部５５などを有している。
【０１０８】
入出力部５２は、周囲の映像を取得するためのＣＣＤ（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）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【０１１９】
１  地図生成装置
１１  連続画像取得部
１２  局所特徴量抽出部
１３  特徴量マッチング部
１４  不変特徴量算出部
１５  距離情報取得部
１６  地図生成部
３０  ナビゲーション装置
３１  連続画像取得部
３２  局所特徴量抽出部
３３  特徴量マッチング部
３４  不変特徴量算出部
３５  距離情報取得部
３６  地図生成部
３７  相対座標算出部
４０  局所メトリカル地図格納部
５４１  地図生成モジュール
５４２  経路計画モジュール
５４３  ナビゲーションモジュール
５４４  音声認識モジュール
５０  ロボット
５１  制御部
５５２  入出力部
５５３  駆動部
５５４  電源部
５５５  外部記憶部
５６１  カメラ
５６２  内蔵マイク
５６３  スピーカ
５６５  センサ部
５７１  モータ
５７２  ドライバ
５８１  バッテリ
５８２  バッテリ制御部
【図１】
2012064131.tif 000022
【図２】
2012064131.tif 000023
【図３】
2012064131.tif 000024
【図５】
2012064131.tif 000025
【図６】
2012064131.tif 000026
【図７】
2012064131.tif 000027
【図８】
2012064131.tif 000028
【図９】
2012064131.tif 000029
【図１０】
2012064131.tif 000030
【図１１】
2012064131.tif 000031
【図４】
2012064131.tif 000032
【図１２】
2012064131.tif 000033
【図１３】
2012064131.tif 000034
【図１４】
2012064131.tif 000035
【図１５】
2012064131.tif 000036