歩行ロボット装置及びその制御プログラム
【課題】環境地図の作成と自己位置推定を、求められる精度を確保しつつ、安価に実現することのできる歩行ロボット装置及びその制御プログラムを提供する。
【解決手段】環境地図を作成する機能は、２ヶ所において撮影した地図作成用環境画像から撮影対象物を示す代表点を抽出し、代表点の位置と撮影した位置座標とから撮影対象物の位置座標を求め、撮影した地図作成用環境画像、代表点の位置及び前記撮影対象物の位置座標とを関連付けた環境地図を作成する。自己位置を推定する機能は、任意の位置座標において撮影した位置推定用環境画像中の撮影対象物の代表点に、対応する地図作成用環境画像中の撮影対象物の位置座標を対応させ、交会法によって前記歩行ロボット装置の自己位置を算出する。
【解決手段】環境地図を作成する機能は、２ヶ所において撮影した地図作成用環境画像から撮影対象物を示す代表点を抽出し、代表点の位置と撮影した位置座標とから撮影対象物の位置座標を求め、撮影した地図作成用環境画像、代表点の位置及び前記撮影対象物の位置座標とを関連付けた環境地図を作成する。自己位置を推定する機能は、任意の位置座標において撮影した位置推定用環境画像中の撮影対象物の代表点に、対応する地図作成用環境画像中の撮影対象物の位置座標を対応させ、交会法によって前記歩行ロボット装置の自己位置を算出する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、歩行ロボット装置及びその制御プログラムに関する。
【背景技術】
【０００２】
自律移動する歩行ロボットは、障害物や段差を自律的に回避しながら移動する。そのため歩行ロボットは、移動環境を記述した環境地図を用いて、常に正確な自己位置を算出する必要がある。従来の歩行ロボットでは、移動環境内を正確に移動することが求められていることから、環境地図は高い精度で作成されなければならない。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−３２２１３８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、家庭内での使用を目的としたホームロボットには、必ずしも高性能・高精度であることが求められているわけではなく、必要最低限の機能を備え、必要十分な性能・精度を満たしていれば良い場合もある。
しかしながら、従来の環境地図の作成と自己位置推定では、歩行ロボットに高価なセンサ、測定器を搭載し、負荷の大きい演算処理を行なう必要があったため、小型化や多機能同時処理や低価格化には制限があった。
【０００５】
本発明は、このような従来の状況に鑑みて提案されたものであり、環境地図の作成と自己位置推定を、求められる精度を確保しつつ、簡易に実現することのできる歩行ロボット装置及びその制御プログラムを提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記課題を解決するための本発明は、搭載した一台の撮像装置を用いて撮影した画像に基づいて環境地図を生成し、生成した環境地図を用いて自己位置を推定して、所定領域内を２足歩行する歩行ロボット装置であって、環境地図を作成する機能は、２ヶ所において撮影した地図作成用環境画像から撮影対象物毎に当該撮影対象物を示す代表点を抽出する代表点抽出部と、２ヶ所において撮影した地図作成用環境画像のそれぞれの代表点の位置と、撮影した歩行ロボット装置の位置座標とから前記撮影対象物の位置座標を求める位置座標取得部と、前記撮影した地図作成用環境画像、代表点の位置及び前記撮影対象物の位置座標とを関連付けた環境地図を作成する環境地図作成部とを有し、自己位置を推定する機能は、任意の位置座標において撮影した位置推定用環境画像中の撮影対象物と、前記環境地図に含まれる地図作成用環境画像中の撮影対象物とを対応させる撮影対象物対応部と、位置推定用環境画像中の撮影対象物の代表点に、対応する地図作成用環境画像中の撮影対象物の位置座標を対応させる位置座標対応部と、位置座標が対応した位置推定用環境画像中の撮影対象物の代表点を用いた交会法によって前記歩行ロボット装置の自己位置を算出する自己位置算出部とを有する。
【発明の効果】
【０００７】
この発明によれば、環境地図の作成と自己位置推定を、求められる精度を確保しつつ、安価あるいは小型化や多機能化を実現することのできる歩行ロボット装置及びその制御プログラムを提供することができる。
【図面の簡単な説明】
【０００８】
【図１】ロボット装置の外観構成を示す図。
【図２】ロボット装置の環境地図の作成と自己位置推定を制御する制御ブロックの構成と、制御ブロック間の信号のフローを示す図。
【図３】環境地図を作成する手順を示すフロー図。
【図４】環境地図を作成する方法を説明するための図。
【図５】特徴点関数を撮影画像をＸＹ平面とする三次元直交座標で表した模式図。
【図６】ランドマーク候補領域を説明する図。
【図７】特徴点が重複した場合の処理方法を説明する図。
【図８】ランドマークの重心位置座標を算出する方法を示す図。
【図９】自己位置を推定する手順を示すフロー図。
【図１０】自己位置を推定する方法を説明するための図。
【図１１】現在位置を求める方法を説明するための図。
【発明を実施するための形態】
【０００９】
[第１の実施の形態]
本実施の形態の歩行ロボット装置（以下、ロボット装置という）は、主として人に毎日の生活を楽しんでもらいながら情報やサービスの提供を行うパーソナルホームコンシェルジュとしての働きを担っている。このために、ロボット装置には、「コミュニケーション知能」による自然なコミュニケーションと共に「移動知能」による生活空間内の自由な移動が求められる。従って、生活空間における家具などが複数存在する環境下において目的地まで自律的に移動できなければならない。
【００１０】
図１は、ロボット装置１の外観構成を示す図である。
このロボット装置１は、視覚、聴覚、会話及び２足歩行の機能を備えたヒューマノイド型として構成されている。このような機能を安価に実現するために、複数の汎用技術を組み合わせて高度・高性能な技術を実現するマルチプレックス方式を採用している。例えば、マザーボード、アクチュエータ等ハードウエアには汎用部品を採用し、ソフトウエアで機能を補完するなどの設計思想が採用されている。
【００１１】
ロボット装置１には、カメラ１０が搭載され、ロボット装置１の周囲の環境を撮影する。ロボット装置１は、カメラ１０で撮影された周囲の環境映像に基づいて環境地図の作成と自己位置推定を実行する。さらにロボット装置１は、図１に示すように、複数の圧力センサ、ジャイロ、加速度センサなどを搭載し、各関節軸の動きを制御して安定した２足歩行動作を実現するが、その詳細の説明は省略する。以下の説明においては、このロボット装置１が、制御装置（不図示）の指令に基づいて任意の姿勢をとることができること、また任意の方向に安定した２足歩行動作を行い得ることに留意されたい。
【００１２】
次に、以下の説明において使用される用語を定義する。
＊ランドマーク：環境地図上において目標物として扱われる撮影画像上の領域。
＊撮影対象物：撮影画像内にある物体。
＊ランドマーク対象物：ランドマークに対応する撮影対象物。
＊撮影情報：撮影画像、撮影角度、撮影したロボット位置などを含む情報。
＊特徴情報：撮影画像上の特徴ある点（特徴点）の位置、特徴点の特徴量、特徴点の数学的なパラメータ（特徴点量）などを含む情報。
＊ランドマーク情報：ランドマークごとの撮影情報、特徴情報、領域を表す座標、領域の重心座標などを含む情報。
【００１３】
図２は、ロボット装置１の環境地図の作成と自己位置推定を制御する制御ブロックの構成と、制御ブロック間の信号のフローを示す図である。
【００１４】
ロボット装置１には制御ブロックとして、環境画像撮影部２１、環境画像連結部２２、特徴情報生成部２３、歩行処理部２４、現在位置座標算出部２５、撮影情報生成部２６、ランドマーク候補生成部２７、対応点抽出部３０、ランドマーク生成部３１及びランドマーク対象物重心位置座標算出部３２が備わっている。
【００１５】
また、ロボット装置１には記憶装置４０が設けられている。この記憶装置４０には、環境地図が格納される。環境地図には、詳細は後述するが、生活空間における家具などのランドマークの存在する位置情報、そのランドマークの特徴情報などが記載される。
【００１６】
図２では、環境地図作成機能を実現するためのブロックを開示している、位置Ａとして表される枠内は移動前の位置における制御ブロック間の信号フローを表し、位置Ｂとして表される枠内は移動後の位置における制御ブロック間の信号フローを表している。なお、同一の機能を備えた制御ブロックは同一の参照符号を付している。
【００１７】
位置Ａとして表した枠内において、環境画像撮影部２１は、カメラ１０を用いて撮影した周囲の環境画像を取得する。環境画像連結部２２は、複数の環境画像を連結処理して１枚の画像にする。特徴情報生成部２３は、連結処理された画像から特徴となる情報を抽出する。歩行処理部２４は、歩幅、歩数、旋回角を積算する。現在位置座標算出部２５は、積算された歩幅、歩数、旋回角から現在位置を表す座標値を算出する。
撮影情報生成部２６は、撮影画像、撮影位置、撮影方向などを関連付けた撮影情報を生成する。ランドマーク候補生成部２７は、撮影情報と特徴情報とからランドマーク候補を抽出し、ランドマーク候補情報を出力する。
【００１８】
位置Ｂとして表した枠内において、上述と同様に周囲環境の撮影が行なわれ撮影情報と特徴情報とが生成される。但し、位置Ｂにおいてはランドマーク候補情報は生成されない。ランドマーク候補情報は、移動前の状態において生成される。
【００１９】
対応点抽出部３０は、移動前と移動後との異なる位置で撮影した座標から取得した情報を比較してそれぞれの情報の対応付けを行なう。ランドマーク生成部３１は、互いの情報から類似した特徴を有する領域（ランドマーク）を抽出する。ランドマーク対象物重心位置座標算出部３２は、ランドマークに対応する撮影対象物（ランドマーク対象物）の重心位置座標を算出する。そして、これらのランドマーク情報、ランドマーク対象物重心位置座標は環境地図として記憶部４０に格納される。
【００２０】
図３は、環境地図を作成する手順を示すフロー図である。図４は、環境地図を作成する方法を説明するための図である。図３及び図４を参照しつつ環境地図作成動作について説明する。
ユーザが、ロボット装置１に対して環境地図作成を指示することによって、ロボット装置１は、環境地図作成動作を開始する。ここで、歩行空間内の座標は、所定位置を基準（原点）とした世界座標系で表されている。この座標系では、ロボット装置１の位置は、Ｘ座標、Ｙ座標、ロボット装置１の向きを表す角度情報をパラメータとして（ｘ、ｙ、θ）で表される。ロボット装置１は、自律で移動する。そして、ロボット装置１は、歩数、歩幅、旋回角を積算することにより、現在位置の座標（ｘ、ｙ、θ）を算出する。従って、環境地図作成を指示された時点において、ロボット装置１は、現在位置の座標（ｘ、ｙ、θ）を把握しているものとして以下の手順を説明する。
【００２１】
図３のステップＳ０１において、ロボット装置１は、現在位置の周囲を撮影して特徴のある対象物をランドマーク候補として抽出する。このステップＳ０１の処理は次のように実施する。
【００２２】
（１００）現在位置座標算出部２５が、ロボット装置１の現在位置Ａ（Ｘ１、Ｙ１、θ１）を取得する。
（１０１）環境画像撮影部２１は、ロボット装置１の頭部のカメラ１０で、現在位置Ａの周囲を撮影する。撮影枚数には特に制限がなく、カメラの回動する範囲、画角等から定められる。全周囲を撮影することが望ましいが、進行方向を中心にして左右側にそれぞれ２枚、合計で４枚撮影でも十分である。図４では、移動方向を基準線として、左側１枚（θα）、右側２枚（θβ、θγ）の３枚が撮影されている。
（１０２）撮影情報生成部２６は、撮影した画像、撮影位置の座標、撮影方向を関連付けて撮影情報として記憶部４０に格納する。
【００２３】
（１０３）環境画像連結部２２は、撮影範囲が連続している画像同士を水平方向につなぎ合わせて１枚の画像とする。
【００２４】
（１０４）特徴情報生成部２３は、撮影した画像から特徴点を探索する。特徴点とは、照明などの環境変化や物体の見え方の違いなど様々な不確定要素に対して不変である特徴量を持つ点あって、対象物の特徴の抽出に適した点のことである。なお、特徴量として一般的に知られているものには、形状やテクスチャ（物の表面の質感）などに基づく特徴量や、画像から特徴点を抽出してその点での特徴ベクトルを特徴量として抽出したＳＩＦＴ特徴量やＳＵＲＦ特徴量などがある。
特徴情報生成部２３は、探索した特徴点毎に特徴量と特徴点量とを算出し、特徴情報として記憶部４０に記憶する。ＳＵＲＦによる特徴量抽出では、特徴量として６４次元あるいは１２８次元のパラメータをもつ特徴ベクトルを得る。また、特徴点量として、画像内の特徴点の座標（Ｘ，Ｙ）、Ｌａｐｌａｃｉａｎ（その点におけるラプラシアンの符号）、Ｓｉｚｅ（特徴のサイズ）、Ｄｉｒ（特徴の方向）、Ｈｅｓｓｉａｎ（ヘシアンの値：特徴の強さを推定できる。）を得る。
【００２５】
（１０５）ランドマーク候補生成部２７は、記憶部４０に格納されている撮影情報及び特徴情報に基いて、ランドマーク候補となる対象物の領域を抽出する。ランドマーク候補の領域を抽出する処理は次のように実施される。
【００２６】
（１０５−１）それぞれの特徴点ｉに対して式（１）で表される特徴点関数Ｚ（ｉ）を定義する。
【数１】
【００２７】
なお、撮影画像内に設定した座標系での特徴点iの座標を(Xi,Yi,0)、特徴点iにおける特徴のサイズをS(i)、特徴点iにおける特徴のヘシアンをH(i)としている。
【００２８】
図５は、特徴点関数を撮影画像をＸＹ平面とする三次元直交座標で表した模式図である。特徴点関数は、ＸＹ平面上では、特徴点を中心として直径がＳｉｚｅの円に投影される立体形状である。特徴点関数の値は、特徴点において最大値（＝ヘシアンの値）となり、特徴点から離れるにつれて余弦関数に従って減少し、円領域の境界点において最小値（＝０）となる。なお、特徴点から円領域の境界点へ向かって減少するカーブは、余弦関数に限られない。単調に減少する関数であれば良い。
【００２９】
（１０５−２）全特徴点関数の値を加算し、その加算された値が所定の閾値（Ｔ）以上の領域を撮影画像内のＸＹ平面に投影する。この領域を「ランドマーク候補領域」という。
図６は、ランドマーク候補領域を説明する図である。図６（１）は、ＸＹ平面上の１つの直線上にある特徴点関数の値を加算して閾値以上となる範囲を示す図である。図６（１）では、Ｌ１からＬ２の範囲が求められる。またこの範囲に属する特徴点も求められる。図６（２）は、特徴点関数の値を加算した値が閾値以上となる範囲をＸＹ平面に表した図である。
【００３０】
（１０５−３）ランドマーク候補領域に外接し、Ｘ軸、Ｙ軸と平行な長方形Ｆを求める。この長方形Ｆで囲まれる領域を「ランドマーク候補」という。また、この長方形Ｆで囲まれる領域に対応する撮影対象物を「ランドマーク候補対象物」という。次に、この長方形Ｆの重心座標、即ち、対角線の交点座標を求める。
【００３１】
（１０５−４）長方形で囲まれる領域に含まれる少なくとも一つの特徴点の特徴量と特徴点量、長方形の頂点座標、重心座標、撮影情報を対応付けて、ランドマーク候補情報として記憶部４０に格納する。
【００３２】
図３のステップＳ０２において、ロボット装置１は、環境地図作成領域内の異なる位置（以下、「位置Ｂ」という。）へ移動する。現在位置座標算出部２５は、歩数、歩幅、旋回角の積算（オドメトリ）により現在位置Ｂの座標（ｘ２、ｙ２、θ２）を算出して取得する。
【００３３】
ステップＳ０３において、ロボット装置１の頭部のカメラ１０で、現在位置Ｂの周囲を撮影する。撮影枚数には特に制限がなく、カメラの回動する範囲、画角等から定められる。例えば、進行方向を中心にして左右側にそれぞれ２枚、合計で４枚撮影する。図４では、移動方向に対して、左側１枚（θδ）、右側２枚（θε、θζ）の３枚が撮影されている。
そして、ロボット装置１は撮影した画像より特徴量、特徴点量を抽出するが、この処理は、ステップ０１の処理（１０４）と同じであるため詳細の説明は省略する。
【００３４】
ステップＳ０４において、対応点抽出部３０は、移動前後の撮影画像から同じ対象物を対応付ける。このステップＳ０４の処理は次のように実施する。なお、以下の説明では、ロボット装置１が位置Ａで撮影した画像を画像Ａと呼び、位置Ｂで撮影した画像を画像Ｂと呼ぶ。
【００３５】
（２００）画像Ａの長方形で囲まれた領域から特徴点を任意に１点選択する。また画像Ｂから特徴点を任意に１点選択する。この画像Ｂの特徴点を選択する際には、それぞれの特徴点の特徴点量Laplacianが同一のものを選択する。Laplacianが異なる特徴同士はマッチしないためである。
【００３６】
（２０１）選択された２つの特徴点について、特徴量の全パラメータに基づいてユークリッド距離を式（２）で算出する。
【数２】
【００３７】
（２０２）ユークリッド距離が閾値以下の場合、それら特徴点の組み合わせと算出したユークリッド距離とを関連付けて記憶部４０に記憶する。
（２０３）画像Ｂにある全ての特徴点について、処理（２０１）、（２０２）を実行し、最小のユークリッド距離を与える特徴点の組み合わせを求める。
【００３８】
（２０４）画像Ａの長方形の内部にある特徴点すべてに対して処理（２０１）〜（２０３）を実行し、最小のユークリッド距離を与える特徴点の組み合わせを求める。この際、画像Ｂ上の特徴点が重複して関連付けられることを回避する。
【００３９】
図７は、特徴点が重複した場合の処理方法を説明する図である。
画像Ａ上の特徴点ａ１と画像Ｂ上の特徴点ｂ１とがユークリッド距離＝１００で対応付けられていたとする。対応付け処理において、画像Ａ上の特徴点ａ２と画像Ｂ上の特徴点ｂ１とがユークリッド距離＝１２０で対応付けられた場合、特徴点ｂ１が重複して対応付けられる。このときは、ユークリッド距離が小さいほうの組み合わせが採用され、ユークリッド距離が大きい組み合わせが棄却される。その結果、画像Ａ上の特徴点ａ２は、ユークリッド距離が２番目に小さい組み合わせである、画像Ｂ上の特徴点ｂ２と対応付けられる。
【００４０】
図３のステップＳ０５において、ランドマーク生成部３１は、ランドマークを抽出する。このステップＳ０５の処理は次のように実施する。
（３００）画像Ｂ上にＸＹ直交座標系を設定し、画像Ｂ上の対応付けられた特徴点の座標を決定する。
【００４１】
（３０１）画像Ａ上と画像Ｂ上の対応付けられた特徴点を任意に４組選択し、その４組を用いて画像Ａと画像Ｂとを一致させるための仮の射影変換行列を求める。
（３０２）画像Ａ上の特徴点の座標を仮の射影変換行列を用いて、画像Ｂ上の座標に変換し、対応する特徴点の座標との距離を求める。そして、距離が所定値以下の組数を、前記仮の射影変換行列のスコアとする。
【００４２】
（３０３）画像Ａ上と画像Ｂ上の対応付けられた特徴点の４組を選択する全ての組み合わせについて、処理（３０１）〜（３０２）を実行して仮の射影変換行列のスコアを求める。
（３０４）最もスコアが高い仮の変換行列を、画像Ａの座標系と画像Ｂの座標系とを一致させるための真の変換行列とする。
【００４３】
（３０５）真の変換行列を用いて画像Ａの長方形を画像Ｂ上に射影する。画像Ｂ上では長方形は四辺形として射影される。
画像Ａ上の現在選択されている長方形で囲まれる領域、及び、これに対応する画像Ｂ上の四辺形で囲まれる領域をランドマークという。
前記長方形で囲まれる領域に対応する撮影対象物と前記四辺形で囲まれる領域に対応する撮影対象物は、それぞれ位置Ａ、位置Ｂから撮影した同一の撮影対象物であって、この撮影対象物をランドマーク対象物という。
【００４４】
（３０６）現在選択されている長方形で囲まれる領域に含まれる特徴点の特徴量と特徴点量、前記長方形の画像Ａの座標系での頂点座標と重心位置座標、撮影情報Ａ、前記長方形に対応する四辺形で囲まれる領域に含まれる特徴点の特徴量と特徴点量、前記四辺形の画像Ｂの座標系での頂点座標と重心座標、撮影情報Ｂを関連付けてランドマーク情報として記憶部４０に記憶する。
【００４５】
図３のステップＳ０６において、ランドマーク対象物重心位置座標算出部３２は、ランドマークの重心位置座標を算出する。このステップＳ０６の処理は次のように実施する。
図８は、ランドマークの重心位置座標を算出する方法を示す図である。
【００４６】
（４００）撮影情報Ａと撮影情報Ｂを基に、位置Ａと位置Ｂを結ぶ線分（以下、「線分ＡＢ」という。）の長さを算出する。
【００４７】
（４０１）現在選択されているランドマーク対象物をＬＭαとし、位置Ａからの撮影により設定されたＬＭαの重心位置を位置α‘としたとき、そのランドマーク情報を基に、位置Ａと位置α‘を結ぶ線分（以下、「直線Ａα‘」という。）と、線分ＡＢとのなす角θαを算出する。
【００４８】
（４０２）位置Ｂからの撮影により設定されたＬＭαの重心位置を位置α“としたとき、そのランドマーク情報を基に、位置Ｂと位置α“を結ぶ線分（以下、「直線Ｂα“」という。）と、線分ＡＢとのなす角θδを算出する。
【００４９】
（４０３）三角測量法に基づき、直線Ａα‘と直線Ｂα“の交点の位置座標を、現在選択されているランドマーク対象物の重心位置の仮の座標として算出する。
【００５０】
（４０４）現在選択されているランドマーク対象物の重心位置の仮の座標が、位置Ａ及び位置Ｂ以外の位置での環境地図作成においても算出され、既に記憶されている場合は、全ての仮の座標の相加平均を、現在選択されているランドマーク対象物の重心位置の新たな仮の座標として算出する。なお、当該ランドマーク対象物が同一か否かは、ランドマーク対象物の重心位置の差（距離）が所定値以内であるかどうかで判断する。
【００５１】
図３のステップＳ０７において、ランドマーク対象物重心位置座標算出部３２は、算出されたランドマーク対象物の重心位置の仮の座標を記憶部４０に記憶する。
【００５２】
ステップＳ０８において、更なる移動先（ロボット位置）が規定されている場合（ステップＳ０８ Ｎｏ）は、ステップＳ０９において、当該移動先において、ステップＳ０１からの処理を繰り返して実行する。そして、規定されている所定の位置において、処理を実行した後は、ランドマーク対象物の重心位置の最終的な仮の座標を、そのランドマーク対象物の重心位置の真の座標として、そのランドマーク情報と関連付けて環境地図に保存する。
【００５３】
なお、上述の形態では、２点のロボット位置で環境地図を作成したが、Ｌ字型（又は逆Ｌ字型）に移動して３点のロボット位置で環境地図を作成してもよく、コの字型（又は逆コの字型）に移動して４点のロボット位置で環境地図を作成してもよい。更に、これらの動作を任意の数組み合わせた複数点のロボット位置で環境地図を作成してもよい。移動経路は、ロボット装置１が適用される環境に合わせて設定しても良い。また、ステップＳ０９において、ロボット装置１は移動せずに、位置Ｂその場において向きを変え位置Ｂを新たな位置Ａとしてこれら手順を繰り返して周囲全体の地図を作成しても良い。結果として、Ｌ字型、コの字型に移動したものと同様の情報を得ることができる。ステップＳ０９の移動はこのような動作も含んだ概念として記載している。
【００５４】
また、上述の形態では、ロボット位置Ａで長方形を形成し、ロボット位置Ｂで四辺形を形成した。即ち、ロボット位置Ａで撮影した画像を基準として、ロボット位置Ｂで撮影した画像との比較でランドマークを抽出した。そのため、ロボット位置Ａでは撮影されておらず、ロボット位置Ｂで初めて撮影された対象物のランドマークは抽出されない。
従って、例えば、ロボット位置Ａ→Ｂ→Ｃ→Ｄと移動する場合において、ロボット位置Ａを基準としてロボット位置Ｂとの比較でランドマークを抽出したあとは、ロボット位置Ｂを基準としてロボット位置Ｃとの比較でランドマークを抽出し、ロボット位置Ｃを基準としてロボット位置Ｄとの比較でランドマークを抽出しても良い。
【００５５】
続いて、上述のようにして作成した環境地図を用いて自己位置を推定する方法を説明する。
図９は、自己位置を推定する手順を示すフロー図である。図１０は、自己位置を推定する方法を説明するための図である。図９及び図１０を参照しつつ自己位置を推定する動作について説明する。
ロボット装置１は、ユーザによって移動空間内の任意の位置に設置され動作開始を指示されたときに現在自身が存在する位置（自己位置）を推定する。更に、ロボット装置１は、所定距離移動したときに自己位置を推定する。従って、ロボットが自己位置を推定する際には、オドメトリにより自己位置が把握できる場合と、自己位置がまったく不明である場合の２つの場合が存在する。図９、図１０に示す自己位置を推定する方法では、この２つの場合における自己位置推定方法を開示する。
【００５６】
ステップＴ０１において、ロボット装置１は、位置認識フラグをセットして、自己位置推定動作を開始する。オドメトリにより自己位置が把握できる場合には位置認識フラグをオンとし、自己位置がまったく不明である場合には位置認識フラグをオフとする。
【００５７】
ステップＴ０２において、ロボット装置１は、周囲を撮影して環境地図にあるランドマークと同一のランドマークを抽出する。このステップＳＴ０２の処理は次のように実施する。
【００５８】
（５００）環境画像撮影部２１は、ロボット装置１の頭部のカメラ１０で、現在位置（位置Ｃ）の周囲を撮影する。撮影枚数には制限が設けられない。撮影された複数枚の画像を撮影範囲の重複が生じないように水平方向につなぎ合わせて、１枚の画像（画像Ｃ）を生成する。
【００５９】
（５０１）画像Ｃから特徴点を探索し、特徴点毎に特徴量と特徴点量（特徴情報Ｃ）を算出する。この処理は、ステップ０１の処理（１０４）と同じであるため詳細の説明は省略する。
【００６０】
（５０２）特徴情報Ｃを参照して、対応点抽出部３０は、算出した特徴点の内、環境地図に保存されているランドマーク上の特徴点と対応する特徴点（対応点）を対応付ける。この処理は、図３のステップＳ０４の処理（２００）〜（２０４）と同じであるため詳細の説明は省略する。
【００６１】
（５０３）なお、対応点が存在していない場合は、現在の位置が不適当な場合であるため、位置を変更する。例えば、特徴点が画像の端部に存在している場合は、向きをその端部の方向に変更する。また特徴点が存在していない場合は、向きを後方に変更する。そして、処理（５００）〜（５０２）を実行する。
【００６２】
（５０４）抽出された対応点を基にして、環境地図に保存されているランドマークと類似した特徴を有する領域を、画像Ｃから抽出する。この処理は、ステップＳ０５の処理（３００）〜（３０６）と同じであるため詳細の説明は省略する。
ここで抽出された領域は、環境地図に保存されているランドマークを画像Ｃの座標系へ射影変換した形状である。つまり、抽出された領域に対応する撮影対象物は、環境地図を作成する際に設定されたランドマーク対象物と同一であり、抽出された領域に対応する撮影対象物は、位置Ｃから撮影したランドマーク対象物である。実際は、対応点抽出や射影変換などの誤差により、抽出された領域に対応する撮影対象物の位置と設定されたランドマーク対象物の位置とは若干異なることがある。
【００６３】
図９のステップＴ０３において、環境地図から前記領域に対応する撮影対象物の重心の位置座標を得る。このようにして、画像Ｃ中に存在するランドマークの座標を環境地図から得る。
【００６４】
図９のステップＴ０４において、ロボット装置１の現在位置Ｃを平板測量でよく用いられる交会法の応用によって求める。このステップＴ０４の処理は次のように実施する。
図１１は、現在位置を求める方法を説明するための図である。
【００６５】
（６００）抽出されたランドマークを任意に２個選択する。図１１に示すように選択した２個のランドマークに対応するランドマーク対象物をＬＭδ、ＬＭεとし、その重心位置をそれぞれ位置δ、位置εとする。環境地図に保存されているランドマーク情報を基に、∠δＣε（＝∠θα）を算出する。
【００６６】
（６０１）位置δ、位置ε及び位置Ｃを通る円の中心座標Ｏαと半径Ｒ１とを求める。∠θαは円の円周角であるため、中心角は２×∠θαとして求められる。従って、この円の中心座標は、位置δ、位置εより等距離（＝Ｒ１）にある頂角が２×∠θαの二等辺三角形の頂点座標として求めることができる。
【００６７】
（６０２）抽出されたｎ個のランドマークの内、２個のランドマークを選択する全組合せ（ｎＣ２通り）について円の中心座標と半径とを求める。
【００６８】
（６０３）２個の円を選択すれば、位置Ｃはその交点座標で表すことができる。そこで、２個の円を選択する全組合せについて位置Ｃの交点座標を求める。そして、これらの相加平均を現在位置Ｃの座標として算出する。
【００６９】
図９のステップＴ０５において、位置認識フラグがＯＮか否かを判断する。
位置認識フラグがＯＦＦの場合（ステップＴ０５ Ｎｏ）、即ち自己位置がまったく不明である場合には、ステップＴ０６において、交会法を応用して求めた現在位置Ｃをロボット装置１の自己位置とする。
そして、自己位置推定処理を終了する。
【００７０】
位置認識フラグがＯＮの場合（ステップＴ０５ Ｎｏ）、即ちオドメトリにより自己位置が把握できる場合には、ステップＴ０７において、オドメトリによる現在位置を算出する。
そして、ステップＴ０８において、オドメトリにより算出された位置座標と交会法で算出された位置座標とを融合して新たに位置座標を算出し、この座標を現在位置Ｃの真の座標とする。
【００７１】
ここで融合とは、オドメトリによる誤差と交会法による誤差をお互いに相殺するように、オドメトリと交会法の両者の値を処理して、より真実に近い値を算出する手法である。融合については、例えば、POEM（Position Estimation module）や、「日本ロボット学会誌 Vol.15 No.8,PP.1180〜1187,1997」などを参考にして行うことができる。
【００７２】
以上、説明した実施の形態によれば、種々の効果を得ることができる。
【００７３】
本実施の形態のロボット装置では、従来のロボットにおいて標準的に装備されていたレーザレンジファインダなどの測距手段は搭載していない。また、周囲環境を撮影するカメラも１台である。従って、ロボット装置のコストを低減することができる。
また、このような低廉なハードウエア構成において、所要の精度を確保するために、環境地図は、ランドマークを表す特徴量、ランドマークを表す重心位置座標などのランドマーク情報の集合で構成している。この結果、画像処理などの処理時間と処理データが少なくなり、負荷の大きい演算処理を排除することが可能となっている。
【００７４】
また、ランドマークの位置をランドマークの特徴量分布から求めた四辺形の重心位置として一点の座標位置で規定している。この結果、自己位置推定処理が簡便化され効率的に処理することが可能となった。
【００７５】
このように、本実施の形態のロボット装置は、ホームユースに求められる精度を確保しつつ安価に実現することのできる歩行ロボット装置として構成されている。
【００７６】
なお、上述の各実施の形態で説明した機能は、ハードウェアを用いて構成するに留まらず、ソフトウェアを用いて各機能を記載したプログラムをコンピュータに読み込ませて実現することもできる。また、各機能は、適宜ソフトウェア、ハードウェアのいずれかを選択して構成するものであっても良い。
【００７７】
尚、本発明は、上記実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。
上記実施形態に開示されている複数の構成要素の適宜な組み合せにより種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除してもよい。更に、異なる実施形態に亘る構成要素を適宜組み合せてもよい。
【符号の説明】
【００７８】
１…ロボット装置、１０…カメラ、２１…環境画像撮影部、２２…環境画像連結部、２３…特徴情報生成部、２４…歩行処理部、２５…現在位置座標算出部、２６…撮影情報生成部、２７…ランドマーク候補生成部、３０…対応点抽出部、３１…ランドマーク生成部、３２…ランドマーク対象物重心位置座標算出部、４０…記憶部。
【技術分野】
【０００１】
本発明は、歩行ロボット装置及びその制御プログラムに関する。
【背景技術】
【０００２】
自律移動する歩行ロボットは、障害物や段差を自律的に回避しながら移動する。そのため歩行ロボットは、移動環境を記述した環境地図を用いて、常に正確な自己位置を算出する必要がある。従来の歩行ロボットでは、移動環境内を正確に移動することが求められていることから、環境地図は高い精度で作成されなければならない。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００７−３２２１３８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、家庭内での使用を目的としたホームロボットには、必ずしも高性能・高精度であることが求められているわけではなく、必要最低限の機能を備え、必要十分な性能・精度を満たしていれば良い場合もある。
しかしながら、従来の環境地図の作成と自己位置推定では、歩行ロボットに高価なセンサ、測定器を搭載し、負荷の大きい演算処理を行なう必要があったため、小型化や多機能同時処理や低価格化には制限があった。
【０００５】
本発明は、このような従来の状況に鑑みて提案されたものであり、環境地図の作成と自己位置推定を、求められる精度を確保しつつ、簡易に実現することのできる歩行ロボット装置及びその制御プログラムを提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記課題を解決するための本発明は、搭載した一台の撮像装置を用いて撮影した画像に基づいて環境地図を生成し、生成した環境地図を用いて自己位置を推定して、所定領域内を２足歩行する歩行ロボット装置であって、環境地図を作成する機能は、２ヶ所において撮影した地図作成用環境画像から撮影対象物毎に当該撮影対象物を示す代表点を抽出する代表点抽出部と、２ヶ所において撮影した地図作成用環境画像のそれぞれの代表点の位置と、撮影した歩行ロボット装置の位置座標とから前記撮影対象物の位置座標を求める位置座標取得部と、前記撮影した地図作成用環境画像、代表点の位置及び前記撮影対象物の位置座標とを関連付けた環境地図を作成する環境地図作成部とを有し、自己位置を推定する機能は、任意の位置座標において撮影した位置推定用環境画像中の撮影対象物と、前記環境地図に含まれる地図作成用環境画像中の撮影対象物とを対応させる撮影対象物対応部と、位置推定用環境画像中の撮影対象物の代表点に、対応する地図作成用環境画像中の撮影対象物の位置座標を対応させる位置座標対応部と、位置座標が対応した位置推定用環境画像中の撮影対象物の代表点を用いた交会法によって前記歩行ロボット装置の自己位置を算出する自己位置算出部とを有する。
【発明の効果】
【０００７】
この発明によれば、環境地図の作成と自己位置推定を、求められる精度を確保しつつ、安価あるいは小型化や多機能化を実現することのできる歩行ロボット装置及びその制御プログラムを提供することができる。
【図面の簡単な説明】
【０００８】
【図１】ロボット装置の外観構成を示す図。
【図２】ロボット装置の環境地図の作成と自己位置推定を制御する制御ブロックの構成と、制御ブロック間の信号のフローを示す図。
【図３】環境地図を作成する手順を示すフロー図。
【図４】環境地図を作成する方法を説明するための図。
【図５】特徴点関数を撮影画像をＸＹ平面とする三次元直交座標で表した模式図。
【図６】ランドマーク候補領域を説明する図。
【図７】特徴点が重複した場合の処理方法を説明する図。
【図８】ランドマークの重心位置座標を算出する方法を示す図。
【図９】自己位置を推定する手順を示すフロー図。
【図１０】自己位置を推定する方法を説明するための図。
【図１１】現在位置を求める方法を説明するための図。
【発明を実施するための形態】
【０００９】
[第１の実施の形態]
本実施の形態の歩行ロボット装置（以下、ロボット装置という）は、主として人に毎日の生活を楽しんでもらいながら情報やサービスの提供を行うパーソナルホームコンシェルジュとしての働きを担っている。このために、ロボット装置には、「コミュニケーション知能」による自然なコミュニケーションと共に「移動知能」による生活空間内の自由な移動が求められる。従って、生活空間における家具などが複数存在する環境下において目的地まで自律的に移動できなければならない。
【００１０】
図１は、ロボット装置１の外観構成を示す図である。
このロボット装置１は、視覚、聴覚、会話及び２足歩行の機能を備えたヒューマノイド型として構成されている。このような機能を安価に実現するために、複数の汎用技術を組み合わせて高度・高性能な技術を実現するマルチプレックス方式を採用している。例えば、マザーボード、アクチュエータ等ハードウエアには汎用部品を採用し、ソフトウエアで機能を補完するなどの設計思想が採用されている。
【００１１】
ロボット装置１には、カメラ１０が搭載され、ロボット装置１の周囲の環境を撮影する。ロボット装置１は、カメラ１０で撮影された周囲の環境映像に基づいて環境地図の作成と自己位置推定を実行する。さらにロボット装置１は、図１に示すように、複数の圧力センサ、ジャイロ、加速度センサなどを搭載し、各関節軸の動きを制御して安定した２足歩行動作を実現するが、その詳細の説明は省略する。以下の説明においては、このロボット装置１が、制御装置（不図示）の指令に基づいて任意の姿勢をとることができること、また任意の方向に安定した２足歩行動作を行い得ることに留意されたい。
【００１２】
次に、以下の説明において使用される用語を定義する。
＊ランドマーク：環境地図上において目標物として扱われる撮影画像上の領域。
＊撮影対象物：撮影画像内にある物体。
＊ランドマーク対象物：ランドマークに対応する撮影対象物。
＊撮影情報：撮影画像、撮影角度、撮影したロボット位置などを含む情報。
＊特徴情報：撮影画像上の特徴ある点（特徴点）の位置、特徴点の特徴量、特徴点の数学的なパラメータ（特徴点量）などを含む情報。
＊ランドマーク情報：ランドマークごとの撮影情報、特徴情報、領域を表す座標、領域の重心座標などを含む情報。
【００１３】
図２は、ロボット装置１の環境地図の作成と自己位置推定を制御する制御ブロックの構成と、制御ブロック間の信号のフローを示す図である。
【００１４】
ロボット装置１には制御ブロックとして、環境画像撮影部２１、環境画像連結部２２、特徴情報生成部２３、歩行処理部２４、現在位置座標算出部２５、撮影情報生成部２６、ランドマーク候補生成部２７、対応点抽出部３０、ランドマーク生成部３１及びランドマーク対象物重心位置座標算出部３２が備わっている。
【００１５】
また、ロボット装置１には記憶装置４０が設けられている。この記憶装置４０には、環境地図が格納される。環境地図には、詳細は後述するが、生活空間における家具などのランドマークの存在する位置情報、そのランドマークの特徴情報などが記載される。
【００１６】
図２では、環境地図作成機能を実現するためのブロックを開示している、位置Ａとして表される枠内は移動前の位置における制御ブロック間の信号フローを表し、位置Ｂとして表される枠内は移動後の位置における制御ブロック間の信号フローを表している。なお、同一の機能を備えた制御ブロックは同一の参照符号を付している。
【００１７】
位置Ａとして表した枠内において、環境画像撮影部２１は、カメラ１０を用いて撮影した周囲の環境画像を取得する。環境画像連結部２２は、複数の環境画像を連結処理して１枚の画像にする。特徴情報生成部２３は、連結処理された画像から特徴となる情報を抽出する。歩行処理部２４は、歩幅、歩数、旋回角を積算する。現在位置座標算出部２５は、積算された歩幅、歩数、旋回角から現在位置を表す座標値を算出する。
撮影情報生成部２６は、撮影画像、撮影位置、撮影方向などを関連付けた撮影情報を生成する。ランドマーク候補生成部２７は、撮影情報と特徴情報とからランドマーク候補を抽出し、ランドマーク候補情報を出力する。
【００１８】
位置Ｂとして表した枠内において、上述と同様に周囲環境の撮影が行なわれ撮影情報と特徴情報とが生成される。但し、位置Ｂにおいてはランドマーク候補情報は生成されない。ランドマーク候補情報は、移動前の状態において生成される。
【００１９】
対応点抽出部３０は、移動前と移動後との異なる位置で撮影した座標から取得した情報を比較してそれぞれの情報の対応付けを行なう。ランドマーク生成部３１は、互いの情報から類似した特徴を有する領域（ランドマーク）を抽出する。ランドマーク対象物重心位置座標算出部３２は、ランドマークに対応する撮影対象物（ランドマーク対象物）の重心位置座標を算出する。そして、これらのランドマーク情報、ランドマーク対象物重心位置座標は環境地図として記憶部４０に格納される。
【００２０】
図３は、環境地図を作成する手順を示すフロー図である。図４は、環境地図を作成する方法を説明するための図である。図３及び図４を参照しつつ環境地図作成動作について説明する。
ユーザが、ロボット装置１に対して環境地図作成を指示することによって、ロボット装置１は、環境地図作成動作を開始する。ここで、歩行空間内の座標は、所定位置を基準（原点）とした世界座標系で表されている。この座標系では、ロボット装置１の位置は、Ｘ座標、Ｙ座標、ロボット装置１の向きを表す角度情報をパラメータとして（ｘ、ｙ、θ）で表される。ロボット装置１は、自律で移動する。そして、ロボット装置１は、歩数、歩幅、旋回角を積算することにより、現在位置の座標（ｘ、ｙ、θ）を算出する。従って、環境地図作成を指示された時点において、ロボット装置１は、現在位置の座標（ｘ、ｙ、θ）を把握しているものとして以下の手順を説明する。
【００２１】
図３のステップＳ０１において、ロボット装置１は、現在位置の周囲を撮影して特徴のある対象物をランドマーク候補として抽出する。このステップＳ０１の処理は次のように実施する。
【００２２】
（１００）現在位置座標算出部２５が、ロボット装置１の現在位置Ａ（Ｘ１、Ｙ１、θ１）を取得する。
（１０１）環境画像撮影部２１は、ロボット装置１の頭部のカメラ１０で、現在位置Ａの周囲を撮影する。撮影枚数には特に制限がなく、カメラの回動する範囲、画角等から定められる。全周囲を撮影することが望ましいが、進行方向を中心にして左右側にそれぞれ２枚、合計で４枚撮影でも十分である。図４では、移動方向を基準線として、左側１枚（θα）、右側２枚（θβ、θγ）の３枚が撮影されている。
（１０２）撮影情報生成部２６は、撮影した画像、撮影位置の座標、撮影方向を関連付けて撮影情報として記憶部４０に格納する。
【００２３】
（１０３）環境画像連結部２２は、撮影範囲が連続している画像同士を水平方向につなぎ合わせて１枚の画像とする。
【００２４】
（１０４）特徴情報生成部２３は、撮影した画像から特徴点を探索する。特徴点とは、照明などの環境変化や物体の見え方の違いなど様々な不確定要素に対して不変である特徴量を持つ点あって、対象物の特徴の抽出に適した点のことである。なお、特徴量として一般的に知られているものには、形状やテクスチャ（物の表面の質感）などに基づく特徴量や、画像から特徴点を抽出してその点での特徴ベクトルを特徴量として抽出したＳＩＦＴ特徴量やＳＵＲＦ特徴量などがある。
特徴情報生成部２３は、探索した特徴点毎に特徴量と特徴点量とを算出し、特徴情報として記憶部４０に記憶する。ＳＵＲＦによる特徴量抽出では、特徴量として６４次元あるいは１２８次元のパラメータをもつ特徴ベクトルを得る。また、特徴点量として、画像内の特徴点の座標（Ｘ，Ｙ）、Ｌａｐｌａｃｉａｎ（その点におけるラプラシアンの符号）、Ｓｉｚｅ（特徴のサイズ）、Ｄｉｒ（特徴の方向）、Ｈｅｓｓｉａｎ（ヘシアンの値：特徴の強さを推定できる。）を得る。
【００２５】
（１０５）ランドマーク候補生成部２７は、記憶部４０に格納されている撮影情報及び特徴情報に基いて、ランドマーク候補となる対象物の領域を抽出する。ランドマーク候補の領域を抽出する処理は次のように実施される。
【００２６】
（１０５−１）それぞれの特徴点ｉに対して式（１）で表される特徴点関数Ｚ（ｉ）を定義する。
【数１】
【００２７】
なお、撮影画像内に設定した座標系での特徴点iの座標を(Xi,Yi,0)、特徴点iにおける特徴のサイズをS(i)、特徴点iにおける特徴のヘシアンをH(i)としている。
【００２８】
図５は、特徴点関数を撮影画像をＸＹ平面とする三次元直交座標で表した模式図である。特徴点関数は、ＸＹ平面上では、特徴点を中心として直径がＳｉｚｅの円に投影される立体形状である。特徴点関数の値は、特徴点において最大値（＝ヘシアンの値）となり、特徴点から離れるにつれて余弦関数に従って減少し、円領域の境界点において最小値（＝０）となる。なお、特徴点から円領域の境界点へ向かって減少するカーブは、余弦関数に限られない。単調に減少する関数であれば良い。
【００２９】
（１０５−２）全特徴点関数の値を加算し、その加算された値が所定の閾値（Ｔ）以上の領域を撮影画像内のＸＹ平面に投影する。この領域を「ランドマーク候補領域」という。
図６は、ランドマーク候補領域を説明する図である。図６（１）は、ＸＹ平面上の１つの直線上にある特徴点関数の値を加算して閾値以上となる範囲を示す図である。図６（１）では、Ｌ１からＬ２の範囲が求められる。またこの範囲に属する特徴点も求められる。図６（２）は、特徴点関数の値を加算した値が閾値以上となる範囲をＸＹ平面に表した図である。
【００３０】
（１０５−３）ランドマーク候補領域に外接し、Ｘ軸、Ｙ軸と平行な長方形Ｆを求める。この長方形Ｆで囲まれる領域を「ランドマーク候補」という。また、この長方形Ｆで囲まれる領域に対応する撮影対象物を「ランドマーク候補対象物」という。次に、この長方形Ｆの重心座標、即ち、対角線の交点座標を求める。
【００３１】
（１０５−４）長方形で囲まれる領域に含まれる少なくとも一つの特徴点の特徴量と特徴点量、長方形の頂点座標、重心座標、撮影情報を対応付けて、ランドマーク候補情報として記憶部４０に格納する。
【００３２】
図３のステップＳ０２において、ロボット装置１は、環境地図作成領域内の異なる位置（以下、「位置Ｂ」という。）へ移動する。現在位置座標算出部２５は、歩数、歩幅、旋回角の積算（オドメトリ）により現在位置Ｂの座標（ｘ２、ｙ２、θ２）を算出して取得する。
【００３３】
ステップＳ０３において、ロボット装置１の頭部のカメラ１０で、現在位置Ｂの周囲を撮影する。撮影枚数には特に制限がなく、カメラの回動する範囲、画角等から定められる。例えば、進行方向を中心にして左右側にそれぞれ２枚、合計で４枚撮影する。図４では、移動方向に対して、左側１枚（θδ）、右側２枚（θε、θζ）の３枚が撮影されている。
そして、ロボット装置１は撮影した画像より特徴量、特徴点量を抽出するが、この処理は、ステップ０１の処理（１０４）と同じであるため詳細の説明は省略する。
【００３４】
ステップＳ０４において、対応点抽出部３０は、移動前後の撮影画像から同じ対象物を対応付ける。このステップＳ０４の処理は次のように実施する。なお、以下の説明では、ロボット装置１が位置Ａで撮影した画像を画像Ａと呼び、位置Ｂで撮影した画像を画像Ｂと呼ぶ。
【００３５】
（２００）画像Ａの長方形で囲まれた領域から特徴点を任意に１点選択する。また画像Ｂから特徴点を任意に１点選択する。この画像Ｂの特徴点を選択する際には、それぞれの特徴点の特徴点量Laplacianが同一のものを選択する。Laplacianが異なる特徴同士はマッチしないためである。
【００３６】
（２０１）選択された２つの特徴点について、特徴量の全パラメータに基づいてユークリッド距離を式（２）で算出する。
【数２】
【００３７】
（２０２）ユークリッド距離が閾値以下の場合、それら特徴点の組み合わせと算出したユークリッド距離とを関連付けて記憶部４０に記憶する。
（２０３）画像Ｂにある全ての特徴点について、処理（２０１）、（２０２）を実行し、最小のユークリッド距離を与える特徴点の組み合わせを求める。
【００３８】
（２０４）画像Ａの長方形の内部にある特徴点すべてに対して処理（２０１）〜（２０３）を実行し、最小のユークリッド距離を与える特徴点の組み合わせを求める。この際、画像Ｂ上の特徴点が重複して関連付けられることを回避する。
【００３９】
図７は、特徴点が重複した場合の処理方法を説明する図である。
画像Ａ上の特徴点ａ１と画像Ｂ上の特徴点ｂ１とがユークリッド距離＝１００で対応付けられていたとする。対応付け処理において、画像Ａ上の特徴点ａ２と画像Ｂ上の特徴点ｂ１とがユークリッド距離＝１２０で対応付けられた場合、特徴点ｂ１が重複して対応付けられる。このときは、ユークリッド距離が小さいほうの組み合わせが採用され、ユークリッド距離が大きい組み合わせが棄却される。その結果、画像Ａ上の特徴点ａ２は、ユークリッド距離が２番目に小さい組み合わせである、画像Ｂ上の特徴点ｂ２と対応付けられる。
【００４０】
図３のステップＳ０５において、ランドマーク生成部３１は、ランドマークを抽出する。このステップＳ０５の処理は次のように実施する。
（３００）画像Ｂ上にＸＹ直交座標系を設定し、画像Ｂ上の対応付けられた特徴点の座標を決定する。
【００４１】
（３０１）画像Ａ上と画像Ｂ上の対応付けられた特徴点を任意に４組選択し、その４組を用いて画像Ａと画像Ｂとを一致させるための仮の射影変換行列を求める。
（３０２）画像Ａ上の特徴点の座標を仮の射影変換行列を用いて、画像Ｂ上の座標に変換し、対応する特徴点の座標との距離を求める。そして、距離が所定値以下の組数を、前記仮の射影変換行列のスコアとする。
【００４２】
（３０３）画像Ａ上と画像Ｂ上の対応付けられた特徴点の４組を選択する全ての組み合わせについて、処理（３０１）〜（３０２）を実行して仮の射影変換行列のスコアを求める。
（３０４）最もスコアが高い仮の変換行列を、画像Ａの座標系と画像Ｂの座標系とを一致させるための真の変換行列とする。
【００４３】
（３０５）真の変換行列を用いて画像Ａの長方形を画像Ｂ上に射影する。画像Ｂ上では長方形は四辺形として射影される。
画像Ａ上の現在選択されている長方形で囲まれる領域、及び、これに対応する画像Ｂ上の四辺形で囲まれる領域をランドマークという。
前記長方形で囲まれる領域に対応する撮影対象物と前記四辺形で囲まれる領域に対応する撮影対象物は、それぞれ位置Ａ、位置Ｂから撮影した同一の撮影対象物であって、この撮影対象物をランドマーク対象物という。
【００４４】
（３０６）現在選択されている長方形で囲まれる領域に含まれる特徴点の特徴量と特徴点量、前記長方形の画像Ａの座標系での頂点座標と重心位置座標、撮影情報Ａ、前記長方形に対応する四辺形で囲まれる領域に含まれる特徴点の特徴量と特徴点量、前記四辺形の画像Ｂの座標系での頂点座標と重心座標、撮影情報Ｂを関連付けてランドマーク情報として記憶部４０に記憶する。
【００４５】
図３のステップＳ０６において、ランドマーク対象物重心位置座標算出部３２は、ランドマークの重心位置座標を算出する。このステップＳ０６の処理は次のように実施する。
図８は、ランドマークの重心位置座標を算出する方法を示す図である。
【００４６】
（４００）撮影情報Ａと撮影情報Ｂを基に、位置Ａと位置Ｂを結ぶ線分（以下、「線分ＡＢ」という。）の長さを算出する。
【００４７】
（４０１）現在選択されているランドマーク対象物をＬＭαとし、位置Ａからの撮影により設定されたＬＭαの重心位置を位置α‘としたとき、そのランドマーク情報を基に、位置Ａと位置α‘を結ぶ線分（以下、「直線Ａα‘」という。）と、線分ＡＢとのなす角θαを算出する。
【００４８】
（４０２）位置Ｂからの撮影により設定されたＬＭαの重心位置を位置α“としたとき、そのランドマーク情報を基に、位置Ｂと位置α“を結ぶ線分（以下、「直線Ｂα“」という。）と、線分ＡＢとのなす角θδを算出する。
【００４９】
（４０３）三角測量法に基づき、直線Ａα‘と直線Ｂα“の交点の位置座標を、現在選択されているランドマーク対象物の重心位置の仮の座標として算出する。
【００５０】
（４０４）現在選択されているランドマーク対象物の重心位置の仮の座標が、位置Ａ及び位置Ｂ以外の位置での環境地図作成においても算出され、既に記憶されている場合は、全ての仮の座標の相加平均を、現在選択されているランドマーク対象物の重心位置の新たな仮の座標として算出する。なお、当該ランドマーク対象物が同一か否かは、ランドマーク対象物の重心位置の差（距離）が所定値以内であるかどうかで判断する。
【００５１】
図３のステップＳ０７において、ランドマーク対象物重心位置座標算出部３２は、算出されたランドマーク対象物の重心位置の仮の座標を記憶部４０に記憶する。
【００５２】
ステップＳ０８において、更なる移動先（ロボット位置）が規定されている場合（ステップＳ０８ Ｎｏ）は、ステップＳ０９において、当該移動先において、ステップＳ０１からの処理を繰り返して実行する。そして、規定されている所定の位置において、処理を実行した後は、ランドマーク対象物の重心位置の最終的な仮の座標を、そのランドマーク対象物の重心位置の真の座標として、そのランドマーク情報と関連付けて環境地図に保存する。
【００５３】
なお、上述の形態では、２点のロボット位置で環境地図を作成したが、Ｌ字型（又は逆Ｌ字型）に移動して３点のロボット位置で環境地図を作成してもよく、コの字型（又は逆コの字型）に移動して４点のロボット位置で環境地図を作成してもよい。更に、これらの動作を任意の数組み合わせた複数点のロボット位置で環境地図を作成してもよい。移動経路は、ロボット装置１が適用される環境に合わせて設定しても良い。また、ステップＳ０９において、ロボット装置１は移動せずに、位置Ｂその場において向きを変え位置Ｂを新たな位置Ａとしてこれら手順を繰り返して周囲全体の地図を作成しても良い。結果として、Ｌ字型、コの字型に移動したものと同様の情報を得ることができる。ステップＳ０９の移動はこのような動作も含んだ概念として記載している。
【００５４】
また、上述の形態では、ロボット位置Ａで長方形を形成し、ロボット位置Ｂで四辺形を形成した。即ち、ロボット位置Ａで撮影した画像を基準として、ロボット位置Ｂで撮影した画像との比較でランドマークを抽出した。そのため、ロボット位置Ａでは撮影されておらず、ロボット位置Ｂで初めて撮影された対象物のランドマークは抽出されない。
従って、例えば、ロボット位置Ａ→Ｂ→Ｃ→Ｄと移動する場合において、ロボット位置Ａを基準としてロボット位置Ｂとの比較でランドマークを抽出したあとは、ロボット位置Ｂを基準としてロボット位置Ｃとの比較でランドマークを抽出し、ロボット位置Ｃを基準としてロボット位置Ｄとの比較でランドマークを抽出しても良い。
【００５５】
続いて、上述のようにして作成した環境地図を用いて自己位置を推定する方法を説明する。
図９は、自己位置を推定する手順を示すフロー図である。図１０は、自己位置を推定する方法を説明するための図である。図９及び図１０を参照しつつ自己位置を推定する動作について説明する。
ロボット装置１は、ユーザによって移動空間内の任意の位置に設置され動作開始を指示されたときに現在自身が存在する位置（自己位置）を推定する。更に、ロボット装置１は、所定距離移動したときに自己位置を推定する。従って、ロボットが自己位置を推定する際には、オドメトリにより自己位置が把握できる場合と、自己位置がまったく不明である場合の２つの場合が存在する。図９、図１０に示す自己位置を推定する方法では、この２つの場合における自己位置推定方法を開示する。
【００５６】
ステップＴ０１において、ロボット装置１は、位置認識フラグをセットして、自己位置推定動作を開始する。オドメトリにより自己位置が把握できる場合には位置認識フラグをオンとし、自己位置がまったく不明である場合には位置認識フラグをオフとする。
【００５７】
ステップＴ０２において、ロボット装置１は、周囲を撮影して環境地図にあるランドマークと同一のランドマークを抽出する。このステップＳＴ０２の処理は次のように実施する。
【００５８】
（５００）環境画像撮影部２１は、ロボット装置１の頭部のカメラ１０で、現在位置（位置Ｃ）の周囲を撮影する。撮影枚数には制限が設けられない。撮影された複数枚の画像を撮影範囲の重複が生じないように水平方向につなぎ合わせて、１枚の画像（画像Ｃ）を生成する。
【００５９】
（５０１）画像Ｃから特徴点を探索し、特徴点毎に特徴量と特徴点量（特徴情報Ｃ）を算出する。この処理は、ステップ０１の処理（１０４）と同じであるため詳細の説明は省略する。
【００６０】
（５０２）特徴情報Ｃを参照して、対応点抽出部３０は、算出した特徴点の内、環境地図に保存されているランドマーク上の特徴点と対応する特徴点（対応点）を対応付ける。この処理は、図３のステップＳ０４の処理（２００）〜（２０４）と同じであるため詳細の説明は省略する。
【００６１】
（５０３）なお、対応点が存在していない場合は、現在の位置が不適当な場合であるため、位置を変更する。例えば、特徴点が画像の端部に存在している場合は、向きをその端部の方向に変更する。また特徴点が存在していない場合は、向きを後方に変更する。そして、処理（５００）〜（５０２）を実行する。
【００６２】
（５０４）抽出された対応点を基にして、環境地図に保存されているランドマークと類似した特徴を有する領域を、画像Ｃから抽出する。この処理は、ステップＳ０５の処理（３００）〜（３０６）と同じであるため詳細の説明は省略する。
ここで抽出された領域は、環境地図に保存されているランドマークを画像Ｃの座標系へ射影変換した形状である。つまり、抽出された領域に対応する撮影対象物は、環境地図を作成する際に設定されたランドマーク対象物と同一であり、抽出された領域に対応する撮影対象物は、位置Ｃから撮影したランドマーク対象物である。実際は、対応点抽出や射影変換などの誤差により、抽出された領域に対応する撮影対象物の位置と設定されたランドマーク対象物の位置とは若干異なることがある。
【００６３】
図９のステップＴ０３において、環境地図から前記領域に対応する撮影対象物の重心の位置座標を得る。このようにして、画像Ｃ中に存在するランドマークの座標を環境地図から得る。
【００６４】
図９のステップＴ０４において、ロボット装置１の現在位置Ｃを平板測量でよく用いられる交会法の応用によって求める。このステップＴ０４の処理は次のように実施する。
図１１は、現在位置を求める方法を説明するための図である。
【００６５】
（６００）抽出されたランドマークを任意に２個選択する。図１１に示すように選択した２個のランドマークに対応するランドマーク対象物をＬＭδ、ＬＭεとし、その重心位置をそれぞれ位置δ、位置εとする。環境地図に保存されているランドマーク情報を基に、∠δＣε（＝∠θα）を算出する。
【００６６】
（６０１）位置δ、位置ε及び位置Ｃを通る円の中心座標Ｏαと半径Ｒ１とを求める。∠θαは円の円周角であるため、中心角は２×∠θαとして求められる。従って、この円の中心座標は、位置δ、位置εより等距離（＝Ｒ１）にある頂角が２×∠θαの二等辺三角形の頂点座標として求めることができる。
【００６７】
（６０２）抽出されたｎ個のランドマークの内、２個のランドマークを選択する全組合せ（ｎＣ２通り）について円の中心座標と半径とを求める。
【００６８】
（６０３）２個の円を選択すれば、位置Ｃはその交点座標で表すことができる。そこで、２個の円を選択する全組合せについて位置Ｃの交点座標を求める。そして、これらの相加平均を現在位置Ｃの座標として算出する。
【００６９】
図９のステップＴ０５において、位置認識フラグがＯＮか否かを判断する。
位置認識フラグがＯＦＦの場合（ステップＴ０５ Ｎｏ）、即ち自己位置がまったく不明である場合には、ステップＴ０６において、交会法を応用して求めた現在位置Ｃをロボット装置１の自己位置とする。
そして、自己位置推定処理を終了する。
【００７０】
位置認識フラグがＯＮの場合（ステップＴ０５ Ｎｏ）、即ちオドメトリにより自己位置が把握できる場合には、ステップＴ０７において、オドメトリによる現在位置を算出する。
そして、ステップＴ０８において、オドメトリにより算出された位置座標と交会法で算出された位置座標とを融合して新たに位置座標を算出し、この座標を現在位置Ｃの真の座標とする。
【００７１】
ここで融合とは、オドメトリによる誤差と交会法による誤差をお互いに相殺するように、オドメトリと交会法の両者の値を処理して、より真実に近い値を算出する手法である。融合については、例えば、POEM（Position Estimation module）や、「日本ロボット学会誌 Vol.15 No.8,PP.1180〜1187,1997」などを参考にして行うことができる。
【００７２】
以上、説明した実施の形態によれば、種々の効果を得ることができる。
【００７３】
本実施の形態のロボット装置では、従来のロボットにおいて標準的に装備されていたレーザレンジファインダなどの測距手段は搭載していない。また、周囲環境を撮影するカメラも１台である。従って、ロボット装置のコストを低減することができる。
また、このような低廉なハードウエア構成において、所要の精度を確保するために、環境地図は、ランドマークを表す特徴量、ランドマークを表す重心位置座標などのランドマーク情報の集合で構成している。この結果、画像処理などの処理時間と処理データが少なくなり、負荷の大きい演算処理を排除することが可能となっている。
【００７４】
また、ランドマークの位置をランドマークの特徴量分布から求めた四辺形の重心位置として一点の座標位置で規定している。この結果、自己位置推定処理が簡便化され効率的に処理することが可能となった。
【００７５】
このように、本実施の形態のロボット装置は、ホームユースに求められる精度を確保しつつ安価に実現することのできる歩行ロボット装置として構成されている。
【００７６】
なお、上述の各実施の形態で説明した機能は、ハードウェアを用いて構成するに留まらず、ソフトウェアを用いて各機能を記載したプログラムをコンピュータに読み込ませて実現することもできる。また、各機能は、適宜ソフトウェア、ハードウェアのいずれかを選択して構成するものであっても良い。
【００７７】
尚、本発明は、上記実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。
上記実施形態に開示されている複数の構成要素の適宜な組み合せにより種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除してもよい。更に、異なる実施形態に亘る構成要素を適宜組み合せてもよい。
【符号の説明】
【００７８】
１…ロボット装置、１０…カメラ、２１…環境画像撮影部、２２…環境画像連結部、２３…特徴情報生成部、２４…歩行処理部、２５…現在位置座標算出部、２６…撮影情報生成部、２７…ランドマーク候補生成部、３０…対応点抽出部、３１…ランドマーク生成部、３２…ランドマーク対象物重心位置座標算出部、４０…記憶部。
【特許請求の範囲】
【請求項１】
搭載した一台の撮像装置を用いて撮影した画像に基づいて環境地図を生成し、生成した環境地図を用いて自己位置を推定して、所定領域内を２足歩行する歩行ロボット装置であって、
環境地図を作成する機能は、
２ヶ所において撮影した地図作成用環境画像から撮影対象物毎に当該撮影対象物を示す代表点を抽出する代表点抽出部と、
２ヶ所において撮影した地図作成用環境画像のそれぞれの代表点の位置と、撮影した歩行ロボット装置の位置座標とから前記撮影対象物の位置座標を求める位置座標取得部と、
前記撮影した地図作成用環境画像、代表点の位置及び前記撮影対象物の位置座標とを関連付けた環境地図を作成する環境地図作成部とを有し、
自己位置を推定する機能は、
任意の位置座標において撮影した位置推定用環境画像中の撮影対象物と、前記環境地図に含まれる地図作成用環境画像中の撮影対象物とを対応させる撮影対象物対応部と、
位置推定用環境画像中の撮影対象物の代表点に、対応する地図作成用環境画像中の撮影対象物の位置座標を対応させる位置座標対応部と、
位置座標が対応した位置推定用環境画像中の撮影対象物の代表点を用いた交会法によって前記歩行ロボット装置の自己位置を算出する自己位置算出部とを有すること
を特徴とする歩行ロボット装置。
【請求項２】
自己位置を推定する機能は、
歩行ロボット装置の移動において、歩数、歩幅、旋回角を積算して自己位置を算出するオドメトリ部と、
前記自己位置算出部が算出した自己位置と前記オドメトリ部が算出した自己位置とを融合して新たな自己位置を算出する融合部とを更に備えること
を特徴とする請求項１に記載の歩行ロボット装置。
【請求項３】
前記撮像装置を設けた頭部を回動して撮影した複数の画像を連結して前記地図作成用環境画像及び位置推定用環境画像を生成する連結部を更に備えることを特徴とする請求項２に記載の歩行ロボット装置。
【請求項４】
環境地図を作成する機能は、環境地図を作成するために歩行と停止とを繰り返して移動する際、停止する毎に地図作成用環境画像を撮影する撮影部を更に備えたことを特徴とする請求項３に記載の歩行ロボット装置。
【請求項５】
搭載した一台の撮像装置を用いて撮影した画像に基づいて環境地図を生成し、生成した環境地図を用いて自己位置を推定して、所定領域内を２足歩行する歩行ロボット装置を制御するプログラムであって、
環境地図を作成する際は、
２ヶ所において撮影した地図作成用環境画像から撮影対象物毎に当該撮影対象物を示す代表点を抽出するステップと、
２ヶ所において撮影した地図作成用環境画像のそれぞれの代表点の位置と、撮影した歩行ロボット装置の位置座標とから前記撮影対象物の位置座標を求めるステップと、
前記撮影した地図作成用環境画像、代表点の位置及び前記撮影対象物の位置座標とを関連付けた環境地図を作成するステップと
を前記歩行ロボットに搭載した制御用コンピュータに実行させ、
自己位置を推定する際は、
任意の位置座標において撮影した位置推定用環境画像中の撮影対象物と、前記環境地図に含まれる地図作成用環境画像中の撮影対象物とを対応させるステップと、
位置推定用環境画像中の撮影対象物の代表点に、対応する地図作成用環境画像中の撮影対象物の位置座標を対応させるステップと、
位置座標が対応した位置推定用環境画像中の撮影対象物の代表点を用いた交会法によって前記歩行ロボット装置の自己位置を算出するステップと
を前記歩行ロボットに搭載した制御用コンピュータに実行させるプログラム。
【請求項１】
搭載した一台の撮像装置を用いて撮影した画像に基づいて環境地図を生成し、生成した環境地図を用いて自己位置を推定して、所定領域内を２足歩行する歩行ロボット装置であって、
環境地図を作成する機能は、
２ヶ所において撮影した地図作成用環境画像から撮影対象物毎に当該撮影対象物を示す代表点を抽出する代表点抽出部と、
２ヶ所において撮影した地図作成用環境画像のそれぞれの代表点の位置と、撮影した歩行ロボット装置の位置座標とから前記撮影対象物の位置座標を求める位置座標取得部と、
前記撮影した地図作成用環境画像、代表点の位置及び前記撮影対象物の位置座標とを関連付けた環境地図を作成する環境地図作成部とを有し、
自己位置を推定する機能は、
任意の位置座標において撮影した位置推定用環境画像中の撮影対象物と、前記環境地図に含まれる地図作成用環境画像中の撮影対象物とを対応させる撮影対象物対応部と、
位置推定用環境画像中の撮影対象物の代表点に、対応する地図作成用環境画像中の撮影対象物の位置座標を対応させる位置座標対応部と、
位置座標が対応した位置推定用環境画像中の撮影対象物の代表点を用いた交会法によって前記歩行ロボット装置の自己位置を算出する自己位置算出部とを有すること
を特徴とする歩行ロボット装置。
【請求項２】
自己位置を推定する機能は、
歩行ロボット装置の移動において、歩数、歩幅、旋回角を積算して自己位置を算出するオドメトリ部と、
前記自己位置算出部が算出した自己位置と前記オドメトリ部が算出した自己位置とを融合して新たな自己位置を算出する融合部とを更に備えること
を特徴とする請求項１に記載の歩行ロボット装置。
【請求項３】
前記撮像装置を設けた頭部を回動して撮影した複数の画像を連結して前記地図作成用環境画像及び位置推定用環境画像を生成する連結部を更に備えることを特徴とする請求項２に記載の歩行ロボット装置。
【請求項４】
環境地図を作成する機能は、環境地図を作成するために歩行と停止とを繰り返して移動する際、停止する毎に地図作成用環境画像を撮影する撮影部を更に備えたことを特徴とする請求項３に記載の歩行ロボット装置。
【請求項５】
搭載した一台の撮像装置を用いて撮影した画像に基づいて環境地図を生成し、生成した環境地図を用いて自己位置を推定して、所定領域内を２足歩行する歩行ロボット装置を制御するプログラムであって、
環境地図を作成する際は、
２ヶ所において撮影した地図作成用環境画像から撮影対象物毎に当該撮影対象物を示す代表点を抽出するステップと、
２ヶ所において撮影した地図作成用環境画像のそれぞれの代表点の位置と、撮影した歩行ロボット装置の位置座標とから前記撮影対象物の位置座標を求めるステップと、
前記撮影した地図作成用環境画像、代表点の位置及び前記撮影対象物の位置座標とを関連付けた環境地図を作成するステップと
を前記歩行ロボットに搭載した制御用コンピュータに実行させ、
自己位置を推定する際は、
任意の位置座標において撮影した位置推定用環境画像中の撮影対象物と、前記環境地図に含まれる地図作成用環境画像中の撮影対象物とを対応させるステップと、
位置推定用環境画像中の撮影対象物の代表点に、対応する地図作成用環境画像中の撮影対象物の位置座標を対応させるステップと、
位置座標が対応した位置推定用環境画像中の撮影対象物の代表点を用いた交会法によって前記歩行ロボット装置の自己位置を算出するステップと
を前記歩行ロボットに搭載した制御用コンピュータに実行させるプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２０１２−２１６０５１（Ｐ２０１２−２１６０５１Ａ）
【公開日】平成２４年１１月８日（２０１２．１１．８）
【国際特許分類】
【出願番号】特願２０１１−８０５７７（Ｐ２０１１−８０５７７）
【出願日】平成２３年３月３１日（２０１１．３．３１）
【出願人】（５９３０５９７７３）富士ソフト株式会社 (28)
【Ｆターム（参考）】
【公開日】平成２４年１１月８日（２０１２．１１．８）
【国際特許分類】
【出願日】平成２３年３月３１日（２０１１．３．３１）
【出願人】（５９３０５９７７３）富士ソフト株式会社 (28)
【Ｆターム（参考）】
[ Back to top ]