JP4942733B2 - 物体認識及び認識された物体を含む周辺環境情報に基づいたロボットの自己位置推定方法 - Google Patents

Description

本発明は移動ロボットが、基準座標が与えられた動作環境内でロボット自ら自身の位置を推定（ｅｓｔｉｍａｔｉｏｎ)する方法に関し、移動ロボットが環境内の個別物体をビジョンセンサにより認識し、認識された物体と認識された物体を含む周辺環境情報を、与えられたマップに基づいて分析して自身の位置を推定する方法に関する。

一般に、産業用ロボットから始まったロボットは、近年は任意の環境でも動作できる知能ロボットの形態へとその応用領域を拡大している。このような知能ロボットとしては、掃除ロボットをはじめ、警備ロボット、危険物探知ロボット、家事ロボット及び教育ロボットなどが挙げられる。このようなロボットが与えられた環境で人間にサービスを提供するためには、ロボット走行技術の確保は必須であり、その中でロボットが自己位置を推定しなければならない自己位置推定（Ｓｅｌｆ−ｌｏｃａｌｉｚａｔｉｏｎ）技術は核心的な要素となっている。

このようなロボットの自己位置推定技術は、基本的に地図に基づいて行われるため、ロボットが利用する地図表象（ｒｅｐｒｅｓｅｎｔａｔｉｏｎ）がいかなる方式で構成されているかと、利用するセンサによって多様に変化し得る。近年の技術の展開は、地図（マップ）は通常格子地図のような数値的地図上に位相的（ｔｏｐｏｌｏｇｉｃａｌ）地図を結合して用い、センサは、レーザ及び超音波センサからビジョンセンサにシフトしているが、これはビジョンセンサが距離情報だけでなく、映像に基づいて色相、物体の識別及び人の識別などのような豊富な追加情報を提供することもできることから、その使用が広がっている。

このようなビジョンセンサ基盤の物体認識技術に基づいてロボットの自己位置を推定しようとした従来技術は、以下の通りである。

韓国特許としては、物体にアクティブタグを付けてこれをビジョンセンサで認識し、これにより、物体とロボット間の位置を測定するための特許（下記の特許文献１参照）と、ビジョン基盤の移動ロボットの位置制御システムに関する特許（下記の特許文献２参照）がある。前者はロボットが移動する経路にある物体に人工的タグを付着しなければならないという不便があり、後者はビジョンセンサがロボットに付着されず、環境に付着されて単にロボットのみを認識してその位置を推定する方式であるので、環境によってビジョンセンサが必要になり、多数のビジョンセンサを設置しなければならないという短所があった。

海外特許としては、米国のビジョンセンサにより自己位置推定とマップの実現を同時に行う特許（下記の特許文献３参照）技術が代表的である。しかしながら、この特許技術も環境でビジョンセンサを用いて物体単位で認識を行わず、３次元上の局所的特徴点を抽出してこれに基づいて自己位置を推定することで、後の環境変化の要因に対処できる手段を喪失して基本的にレーザセンサを利用するのと大きく変わらないという短所があった。

そして、公開された論文のうち、本特許と類似した技術は、以下の通りである。

日本のＹｕｔａなどが物体認識に基づいて物体中心の位相的地図を構成し、ロボットの位置はそのような物体を１つずつ認識すると同時に、ロボットが動きながら、他の物体を認識してロボットの位置を推定する方法を提示した（下記の非特許文献４参照）。このような方法は、ロボットの自己位置推定方法において局所的位置追跡（ｌｏｃａｌ ｌｏｃａｌｉｚａｔｉｏｎ ｔｒａｃｋｉｎｇ）に該当する方法であって、ロボットが任意の位置に突然動かされたり、局所的位置推定に失敗したりするので、新たにロボットの位置を推定しようとする全域的自己位置の推定には適用し難いという問題点があった。

韓国のパク・スンヨンらが室内環境を局所領域中心の位相的地図として表象し、各局所領域に含まれている物体を認識してロボットが位置する局所領域を取得し、その局所領域に定義された局所座標系に対するロボットの位置を計算する方法を提示した。このような方法は、ロボットの位置が認識された物体の３次元情報のみを用いて計算されるので、認識された物体の３次元情報の数が少ない場合には、計算された位置が不正確になり得るという問題点があった。
韓国公開番号１０−２００６−０１０２３８９号 韓国公開番号１０−２００５−００６２７１３号 米国特許ＵＳ７０１５８３１号 Ｍ. Ｔｏｍｏｙｏ ａｎｄ Ｓ. Ｙｕｔａ,"Ｏｂｊｅｃｔ−ｂａｓｅｄ ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ ｍａｐｐｉｎｇ ｕｓｉｎｇ ｌｏｏｐ ｃｏｎｓｔｒａｉｎｔｓ ａｎｄ ｇｅｏｍｅｔｒｉｃ ｐｒｉｏｒ ｋｎｏｗｌｅｄｇｅ," Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ, ｐｐ． ８６２〜８６７, ２００３． Ｓ． Ｐａｒｋ， Ｍ． Ｐａｒｋ, ａｎｄ Ｓ．−Ｋ． Ｐａｒｋ, "Ｖｉｓｉｏｎ−ｂａｓｅｄ Ｇｌｏｂａｌ Ｌｏｃａｌｉｚａｔｉｏｎ ｆｏｒ Ｍｏｂｉｌｅ Ｒｏｂｏｔｓ ｗｉｔｈ ａｎ Ｏｂｊｅｃｔ Ｅｎｔｉｔｙ−ｂａｓｅｄ Ｈｙｂｒｉｄ Ｍａｐ," Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ａｄｖａｎｃｅｄ Ｒｏｂｏｔｉｃｓ, ｐｐ．７６１〜７６６，２００７．

本発明は前記事情に鑑みてなされたものであって、その目的は、ロボットの環境認識のための物体と物体を含む環境情報が予め位相学地図形態で３次元情報と共に格納されている状態でビジョンセンサによって環境内の物体認識を行い、物体が認識される場合に、物体と物体を含む環境の３次元情報を用いてロボットの自己位置を推定する方法を提供することにある。

また、本発明の他の目的は、ビジョンセンサによる物体認識時間はロボットの走行中に位置推定に適用し難いという前提の下で、ロボットが環境の個別的物体を認識し、それに基づいて位置を推定するようにしてロボットが停止している位置で全域的位置推定を行うロボットの自己位置推定方法を提供することにある。

前記目的を達成するために、本発明では第１の側面として、ステレオ映像装置で形成されたカメラ部と、ロボット移動経路周囲の地図が格納されたデータベース部と、前記カメラ部で獲得した映像情報を用いて自身の位置を推定する位置演算部とを有するロボットの自己位置推定方法であって、前記カメラ部でロボット周囲の映像を獲得する第１工程と、前記位置演算部で前記カメラ部が獲得した映像内の個別物体を認識し、個別物体の局所特徴点と個別物体を含む周辺環境の局所特徴点のカメラ座標系位置値を生成する第２工程と、前記位置演算部で前記データベース部に格納されている地図及び前記第２工程の結果に基づいてロボットの位置を推定する第３工程とを含むロボットの自己位置推定方法が提示され、

第１の側面において、前記第３工程は、前記データベースに格納された地図から前記第２工程で認識した物体が１つ以上含まれているノードをロボットが位置し得る候補ノードとして選定する候補ノード選定工程と、前記第２工程で生成された位置値を用いて選定された全ての候補ノードに対するロボットの暫定位置を計算する暫定位置計算工程と、計算された暫定位置を初期位置値として選定された全ての候補ノードにパーティクルフィルタリング技法を適用してロボットの位置を推定する位置推定工程を含んでなることを特徴とし、

第１の側面において、前記第３工程は、前記データベースに格納された地図から前記第２工程で認識した物体が１つ以上含まれているノードをロボットが位置し得る候補ノードとして選定する候補ノード選定工程と、前記第２工程で生成された位置値を用いて選定された全ての候補ノードに対するロボットの暫定位置を計算する暫定位置計算工程と、候補ノードの中から最終候補ノードを選択する最終候補ノード選択工程と、最終候補ノードに対するロボットの暫定位置を初期位置値として最終候補ノードにパーティクルフィルタリング技法を適用してロボットの位置を推定する位置推定工程とを含んでなることを特徴とし、

第２の側面として、３６０°より小さな視野角を有し、ステレオ映像装置で構成されたカメラ部と、前記カメラ部を一定角度ずつ回転させることができる回転装置と、ロボット移動経路周囲の地図を格納しているデータベース部と、前記カメラ部で獲得した映像情報を用いて自身の位置を推定する位置演算部とを有するロボットの自己位置推定方法であって、

前記カメラ部が一定方向のロボット周囲の映像を獲得する第１工程と、前記位置演算部において前記カメラ部が獲得した映像内で認識した物体がない場合には前記回転装置を駆動してカメラを回転させた後、他の方向の映像を獲得する過程を獲得された映像で認識した個別物体が存在するまで繰り返し、認識した個別物体の局所特徴点と個別物体を含む周辺環境の局所特徴点のカメラ座標系位置値を生成する第２工程と、前記位置演算部で前記データベース部に格納された地図から前記認識された物体が１つ以上含まれているノードをロボットが位置し得る候補ノードとして選定する第３工程と、前記位置演算部で候補ノードの優先順位を決定する第４工程と、前記位置演算部で候補ノードに含まれている物体とロボット間の距離及び最優先順位物体を計算する第５工程と、前記位置演算部で前記回転装置を用いてカメラ部を回転させて候補ノードの最優先順位物体を確認する第６工程と、前記位置演算部で前記第６工程で最優先物体が確認されたノードにパーティクルフィルタリング技法を用いてロボットの位置を推定する第７工程とを含むことを特徴とするロボットの自己位置推定方法が提示され、

第２の側面において、前記第６工程は候補ノードの最優先物体が認識できなければ、前記回転装置でカメラ部を回転させて前記第４工程での該当候補ノードのうち、次の優先順位の候補ノードの最優先物体を確認することを特徴とする。

本発明による物体認識及び認識された物体を含む周辺環境情報に基づいたロボットの自己位置推定方法によれば、環境内の個別物体をステレオビジョンセンサにより認識し、それに関する３次元情報だけでなく、その周辺環境の３次元情報を共に用いることで、ロボットの正確な位置を推定できるという効果を奏する。また、任意に位置が変化する物体（例えば、人間、椅子）が多い一般的な環境において更に正確な位置推定を行えるという効果がある。

以下、実施形態を通じて本発明による物体認識及び周辺環境情報に基づいたロボットの自己位置推定システムと方法について更に具体的に説明する。

図１は、本発明による物体認識及び周辺環境情報に基づいて自己位置を推定するロボットの構造図である。

本発明による物体認識及び周辺環境情報に基づいて自己位置を推定するロボットは、図１に示すように、カメラ部１００と、ロボット本体２００と、ロボット移動部３００とを含んで構成される。

前記カメラ部１００は、ロボットの位置を推定するための手段であって、ステレオ映像入力装置で構成される。

前記カメラ部１００は、一定範囲の視野角を有するカメラを用いてもよく、全方向の視野角を有するカメラを用いてもよい。但し、一定範囲の視野角を有するカメラを用いる場合には、図１に示すように、カメラを一定角度ずつ回転させることができるパン／チルト装置１１０を追加してカメラを回転させることで、全方向の映像入力を受けることができる。

前記パン／チルト装置１１０は、水平に左右回転するパンニング（Ｐａｎｎｉｎｇ）と、上下にうなずく動作であるティルティング（ｔｉｌｔｉｎｇ）が可能な回転装置である。

ロボット本体２００は、外部動作を行うための機械アームであるマニピュレータ２１０が外部に装着され、内部にロボット移動経路周囲の地図が格納されたデータベース部（図示せず）と、前記カメラ部１００から入力される映像情報を前記データベース部に格納された地図と照合して物体を認識し、ロボットの自己位置を推定する位置演算部（図示せず）とを含む。

前記データベース部に格納された地図は予め位相学的地図形態に製作され、地図上の各物体の３次元位置及び物体を含む周辺環境の局所特徴点の３次元位置と共に格納される。

ロボット移動部３００は、ロボットを移動経路に沿って移動させる構成であって、輪やキャタピラをはじめとしてロボットの本体を移動させることができるあらゆる装置を用いることができる。

図２は、本発明による物体認識及び周辺環境情報に基づいたロボットの自己位置推定方法を示す基本順序図である。

本発明による物体認識及び周辺環境情報に基づいたロボットの自己位置推定方法は、図２に示すように、カメラ部１００から映像の入力を受ける映像入力工程Ｓ１１０と、前記映像入力工程Ｓ１１０で入力された映像内の個別物体を認識し、また個別物体の３次元位置及び物体を含む周辺環境の局所特徴点の３次元位置情報を生成する物体認識及び情報抽出工程Ｓ１２０と、ロボットの位置推定手段が前記物体認識及び情報抽出工程Ｓ１２０で抽出された情報を用いて予め登録されている物体基盤の地図に基づいてロボットの位置を推定するロボット位置推定工程Ｓ１３０とを含む。従って、本発明では基本的にまず環境内の個別物体を認識し、これに基づいて物体と物体を含む周辺環境の３次元距離情報を抽出してロボットの位置を推定するといえる。

本発明においては、局所特徴点の集合により物体認識を行うが、カメラ部１００を用いて獲得した環境映像から局所特徴点を抽出し、この特徴点と、前記データベース部に格納された物体モデルの局所特徴点とを比較する方法を用いる。

図３は、ロボットが認識した物体４１０と認識された物体を含む周辺環境４００を示す。^ＲＰ_ｉは、図４に示すように、物体と物体を含む周辺環境の局所特徴点とカメラ部１００との間の３次元距離値であり、^ＮＶ_ｉは、図５に示すように、図４での物体及び物体を含む周辺環境の局所特徴点と環境地図中心座標系との間の３次元距離値であり、^ＲＰ_ｉと^ＮＶ_ｉとの関係は、図６に示す通りである。

前記ロボット位置推定工程Ｓ１３０でロボットの位置を推定するのに用いられる地図は、ロボット本体２００のデータベース部に格納された地図であって、図７に示すように、ロボットの走行経路を中心とした位相学的地図である。

図７において、Ａ、Ｂ、Ｃなどがノードに該当し、各ノードは独立した基準座標系を有しており、各ノードを連結するエッジは各ノード座標系の相対的な距離情報を有している。ところが、このような環境地図を考慮すれば、ロボットが物体１つのみを認識してその物体内部の局所特徴点とその物体周辺環境の局所特徴点のみを有してロボットの位置を推定するようになれば、２つ以上の物体に基づいて位置を推定するよりは不正確になってしまうことが分かり、特に、ロボットの回転角度の誤差が大きくならざるを得ないことが分かる。従って、本発明では複数の物体認識に基づいてロボットの位置を推定する。

また、位相学的地図基盤と物体認識中心の位置推定を行う場合、図７に示すように、特定の物体が特定のノードでのみ観測されるのではない。例えば、物体−ａ、物体−ｂ及び物体−ｃなどはノード−Ｌとノード−Ｎでいずれも観測可能である。

即ち、これは物体が観測されることが、ロボットがどのノードの近傍に存在するかを示すとは限らないため、物体の位置を特定させる基準ノードを決定しなければならないという問題もある。本発明ではこのような位置推定の方法を以下のようなパーティクルフィルタリング（Ｍ．Ｉｓａｒｄ ａｎｄ Ａ． Ｂｌａｋｅ, “Ｃｏｎｄｅｎｓａｔｉｏｎ ｌ ｃｏｎｄｉｔｉｏｎａｌ ｄｅｎｓｉｔｙ ｐｒｏｐａｇａｔｉｏｎ ｆｏｒ ｖｉｓｕａｌ ｔｒａｃｋｉｎｇ,” Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆ Ｃｏｍｐｕｔｅｒ Ｖｉｓｉｏｎ ２９（１）, ｐｐ． ５〜２８, １９９８）技法を適用して解決する。

図８は、パーティクルフィルタリングに基づいた位置推定方法を示す順序図である。

このようなパーティクルフィルタリングに基づいた位置推定方法は、図８に示すように、サンプリング工程Ｓ２１０と、誤差計算工程Ｓ２２０と、重み値計算工程Ｓ２３０と、繰り返し条件確認工程Ｓ２４０と、位置推定工程Ｓ２５０とを含む。

前記サンプリング工程Ｓ２１０は、特定物体が観測されれば、該当物体を観測できる所にロボットが存在すると推定されるので、該当物体を観測できるロボットの位置に対して、これをランダムサンプリングして生成する工程である。

前記サンプリング工程Ｓ２１０で繰り返しステップをｔ、ランダムサンプルの番号をｎとすれば、
・ｔ＝１である場合（開始初期）

前記誤差計算工程Ｓ２２０は、前記サンプリング工程Ｓ２１０で生成された位置サンプルにロボットの移動による動作誤差とビジョンセンサの測定誤差及び物体認識のエラーなどにより発生する誤差範囲を下記の式５を用いて適用する工程である。

前記繰り返し条件確認工程Ｓ２４０は、サンプリング工程Ｓ２１０と、誤差計算工程Ｓ２２０と、重み値計算工程Ｓ２３０が一定条件を満たす間に繰り返し行われたかを確認する工程であり、繰り返し条件は均一な一定回数（Ｔ）になることもでき、前記式６でサンプルのうちの最も大きな重み値を有するサンプルの重み値がある閾値（例えば、正規化された場合、０.９）以下であるという条件を用いることもでき、また前記２通りの方法をＯＲで組み合わせてＴ回繰り返したか、最も大きな重み値を有するサンプルの重み値がある閾値（例えば、正規化された場合、０.９）以上であれば、繰り返し実行を停止させることもできる。

前記位置推定工程Ｓ２５０は、サンプルがサンプリング工程Ｓ２１０と誤差計算工程Ｓ２２０と重み値計算工程Ｓ２３０をＴ回繰り返すようになれば、サンプルが１つの候補位置に収斂するので、重み平均を用いてその位置を下記の式８のように推定する工程である。

図９は、本発明の第１実施形態による物体認識及び認識された物体を含む周辺環境情報に基づいたロボットの自己位置推定方法を示す順序図である。

本発明の一実施形態による物体認識及び認識された物体を含む周辺環境情報に基づいたロボットの自己位置推定方法は、カメラ部１００に装着されるセンサが全方向ステレオビジョンセンサである場合の実施形態であって、図９に示すように、全方向環境イメージ及びステレオ深さ情報獲得工程Ｓ３１０と、物体認識工程Ｓ３２０と、認識された物体が１つ以上存在する候補ノード選定工程Ｓ３３０と、全ての候補ノードに対するロボットの３次元位置計算工程Ｓ３４０と、全ての候補ノードにサンプリングしてパーティクル・フィルタリング・アルゴリズムを行う工程Ｓ３５０と、最適ノード及び最適ノードに対するロボットの２次元位置確定工程Ｓ３６０とを含む。

前記全方向環境イメージ及びステレオ深さ情報獲得工程Ｓ３１０は、カメラ部１００がロボット周囲の全方向環境映像及びそれに対してステレオビジョンの基盤の下で距離情報を獲得する工程である。

前記物体認識工程Ｓ３２０は、位置演算部が前記全方向環境イメージ及びステレオ深さ情報獲得工程Ｓ３１０で獲得した全方向環境映像から物体及び物体を含む周辺環境の局所特徴点を抽出し、抽出された局所特徴点の３次元位置値（カメラ座標系基準）をステレオ距離情報を用いて求め、ロボット本体２００のデータベース部に格納されている物体モデルの局所特徴点と比較して物体を認識する工程である。

前記認識された物体が１つ以上存在する候補ノード選定工程Ｓ３３０は、位置演算部がデータベース部に格納された地図において前記物体認識工程Ｓ３２０で認識された物体が１つ以上含まれているノードをロボットが位置し得る候補ノードとして選定する工程である。

前記全ての候補ノードに対するロボットの３次元位置計算工程Ｓ３４０は、位置演算部が全ての候補ノード座標系に対するロボットの暫定位置を計算し、計算されたロボットの位置を用いてノード近接重み値を計算する工程である。

前記全ての候補ノードに対するロボットの３次元位置計算工程Ｓ３４０で計算されるロボットの暫定位置は、前記式１と式２から求めることができ、マッチされた局所的特徴点が３点以上であれば、最小二乗法を用いて計算を行う。

このように得られた暫定位置は、次の工程のパーティクルフィルタリングによって最終的に正確な位置を推定するための初期位置値として用いられる。

前記ノード近接重み値は、物体存在重み値、物体−環境特徴重み値、及び物体−環境距離重み値を用いて計算される値であり、このうち、物体−環境距離重み値を計算するのに各候補ノードに対するロボットの３次元位置が用いられる。

前記物体存在重み値は、候補ノードに含まれている物体のうち、ロボットが認識した物体と重なる数によって変化する重み値であり、下記の式９により計算される。

前記物体−環境特徴重み値は、候補ノードに格納された物体モデルの局所特徴点とその物体を含む周辺環境から抽出した環境モデルの局所特徴点をロボットが認識した物体とその物体を含む周辺環境の局所的特徴点のマッチング程度（マッチされた局所的特徴点の個数など）による重み値であり、下記の式１０により計算される。

前記物体−環境距離重み値は、ロボットが認識した物体及びその物体を含む周辺環境の局所的特徴点の３次元値を候補ノードの同一物体及びその物体を含む周辺環境の局所的特徴点の３次元値で補間（ｆｉｔｔｉｎｇ)し、補間された局所的特徴点の３次元値と候補ノードの同一の局所的特徴点の３次元値間の距離差であり、下記の式１１により計算される。

前記ノード近接重み値は、ロボットのその候補ノードに対する近接程度を示す重み値であり、前記式９、１０、１１により計算された物体存在重み値、物体−環境特徴重み値、物体−環境距離重み値を統合して、下記の式１２により計算される。

全ての候補ノードにサンプリングしてパーティクル・フィルタリング・アルゴリズムを行う工程Ｓ３５０は、位置演算部が前記全ての候補ノードに対するロボットの３次元位置計算工程Ｓ３４０で計算された暫定位置に対して図７に示すパーティクル・フィルタリング・アルゴリズムのうち、サンプリング工程Ｓ２１０と、誤差計算工程Ｓ２２０と、重み値計算工程Ｓ２３０と、繰り返し条件確認工程Ｓ２４０を行い、正確な位置推定に必要な最終重み値を計算する工程である。

そして、ロボットが認識した物体及びその物体を含む環境映像の３次元特徴点^ＲＰ_ｉを下記の式１４を用いて以下の通り候補ノードＮ_ｋに対する値に変換する。

ここで、ｎ_Ｆはロボットが認識した全ての物体及びその物体を含む周辺環境の局所的特徴点と候補ノードＮ_ｋに存在する物体のうち、ロボットが認識した物体と同一物体及びその物体を含む周辺環境の局所的特徴点の間でマッチされた局所的特徴点の個数である。

前記最適ノード及び最適ノードに対するロボットの２次元位置確定工程Ｓ３６０は、図８に示す位置推定工程Ｓ２５０と同じ工程でサンプルが最も多く収斂されたノードのサンプルのみで前記式８を行ってロボットの位置を推定する工程である。

図１０は、本発明の第２実施形態による物体認識及び認識された物体を含む周辺環境情報に基づいたロボットの自己位置推定方法を示す順序図である。

本発明の第２実施形態による物体認識及び認識された物体を含む周辺環境情報に基づいたロボットの自己位置推定方法は、カメラ部１００に装着されるセンサが全方向ステレオビジョンセンサであり、ロボットの属する最終ノードを予め定める場合の実施形態であって、図１０に示すように、全方向環境イメージ及びステレオ深さ情報獲得工程Ｓ４１０と、物体認識工程Ｓ４２０と、認識された物体が１つ以上存在する候補ノード選定工程Ｓ４３０と、全ての候補ノードに対するロボットの３次元位置計算工程Ｓ４４０と、最終候補ノード選定工程Ｓ４５０と、最終候補ノードにサンプリングしてパーティクル・フィルタリング・アルゴリズムを行う工程Ｓ４６０と、最適ノード及び最適ノードに対するロボットの２次元位置確定工程Ｓ４７０とを含む。

全方向環境イメージ及びステレオ深さ情報獲得工程Ｓ４１０と、物体認識工程Ｓ４２０と、認識された物体が１つ以上存在する候補ノード選定工程Ｓ４３０と、全ての候補ノードに対するロボットの３次元位置計算工程Ｓ４４０は、前記第１実施形態の全方向環境イメージ及びステレオ深さ情報獲得工程Ｓ３１０と、物体認識工程Ｓ３２０と、認識された物体が１つ以上存在する候補ノード選定工程Ｓ３３０と、全ての候補ノードに対するロボットの３次元位置計算工程Ｓ３４０と同一であるので、詳細な説明は省略する。

最終候補ノード選定工程Ｓ４５０は、前記全ての候補ノードに対するロボットの３次元位置計算工程Ｓ４４０で計算されたノード近接重み値が最も大きなノードを最終候補ノードとして選定する工程である。

前記最終候補ノードにサンプリングしてパーティクル・フィルタリング・アルゴリズムを行う工程Ｓ４６０は、位置演算部が前記最終候補ノード選定工程Ｓ４５０で選択された最終候補ノードに図８に示すパーティクル・フィルタリング・アルゴリズムのうち、サンプリング工程Ｓ２１０と、誤差計算工程Ｓ２２０と、重み値計算工程Ｓ２３０と繰り返し条件確認工程Ｓ２４０を行い、正確な位置を推定するのに必要な最終重み値を計算する工程である。

前記最適ノード及び最適ノードに対するロボットの２次元位置確定工程Ｓ４７０は、前記最終候補ノード選定工程Ｓ４５０で選択され、前記最終候補ノードにサンプリングしてパーティクル・フィルタリング・アルゴリズムを行う工程Ｓ４６０で最終重み値が計算されたノードに前記式８を行ってロボットの位置を推定する工程である。

図１１A 及び 図１１Bは、本発明の第３実施形態による物体認識及び認識された物体を含む周辺環境情報に基づいたロボットの自己位置推定方法を示す順序図である。

本発明の第３実施形態による物体認識及び認識された物体を含む周辺環境情報に基づいたロボットの自己位置推定方法は、カメラ部１００に装着されるセンサがステレオカメラが３６０°ではない一定の視野角を有するステレオカメラである場合にロボットの位置推定時間を短縮するために、環境で２つの物体を認識する方法に基づいてロボットの位置を推定する実施形態であって、 図１１A 及び 図１１Bに示すように、任意の一方向の環境イメージ及びステレオ深さ情報獲得工程Ｓ５１０と、物体認識工程Ｓ５１５と、認識された物体があるかを確認する工程Ｓ５２０と、認識された物体が１つ以上存在する候補ノード選定工程Ｓ５２５と、全ての候補ノードに対するロボットの３次元位置計算工程Ｓ５３０と、候補ノードの優先順位計算工程Ｓ５３５と、候補ノードに含まれている物体のロボットに対する位置計算及び優先順位計算工程Ｓ５４０と、最優先順位候補ノードの最優先順位物体を眺める工程Ｓ５４５と、その方向の環境イメージ及びステレオ深さ情報獲得工程Ｓ５５０と、物体認識工程Ｓ５５５と、認識された物体が最優先順位物体と一致するかを確認する工程Ｓ５６０と、次の優先順位候補ノードの最優先順位物体を眺める工程Ｓ５６２と、最終ノードにサンプリングしてパーティクル・フィルタリング・アルゴリズムを行う工程Ｓ５６５と、最終ノードに対するロボットの２次元位置確定工程Ｓ５７０とを含む。

前記任意の一方向の環境イメージ及びステレオ深さ情報獲得工程Ｓ５１０は、カメラ部１００が向かっている方向の環境映像及びそれに対してステレオビジョンの基盤の下で距離情報を獲得する工程である。

前記物体認識工程Ｓ５１５は、位置演算部が前記任意の一方向の環境イメージ及びステレオ深さ情報獲得工程Ｓ５１０で獲得した環境映像から物体及び物体を含む周辺環境の局所的特徴点を抽出し、抽出された局所特徴点に対する３次元位置値（カメラ座標系基準）を、ステレオ距離情報を用いて求め、ロボット本体２００のデータベース部に格納された物体モデルの局所特徴点と比較して物体を認識する工程である。

前記認識された物体があるかを確認する工程Ｓ５２０は、位置演算部が前記物体認識工程Ｓ５１５で認識された物体があるかを確認し、認識された物体がなければ、パン／チルト装置１１０を動作してカメラ部１００を回転（ｐａｎｎｉｎｇ）させて他の方向に向かうようにする工程である。

前記認識された物体が１つ以上存在する候補ノード選定工程Ｓ５２５は、位置演算部が前記データベース部に格納された地図において前記物体認識工程Ｓ５１５で認識された物体が１つ以上含まれているノードをロボットが位置し得る候補ノードとして選定する工程である。

前記全ての候補ノードに対するロボットの３次元位置計算工程Ｓ５３０は、前記全ての候補ノードに対するロボットの３次元位置計算工程Ｓ３４０と同一であるので、詳細な説明は省略する。

前記候補ノードの優先順位計算工程Ｓ５３５は、前記全ての候補ノードに対するロボットの３次元位置計算工程Ｓ５３０で計算されたノード近接重み値の値によってそれぞれノードの優先順位を計算する工程である。

前記候補ノードに含まれている物体のロボットに対する位置計算及び優先順位計算工程Ｓ５４０は、位置演算部が、ロボットが認識した物体を除いて、前記認識された物体が１つ以上存在する候補ノード選定工程Ｓ５２５で選択された候補ノードに含まれている物体のロボットに対する位置を計算して、ロボットの暫定位置から最も近い物体を検証物体として定める工程である。

前記最優先順位候補ノードの最優先順位物体を眺める工程Ｓ５４５は、位置演算部がパン／チルト装置１１０を動作して前記候補ノードに含まれている物体のロボットに対する位置計算及び優先順位計算工程Ｓ５４０で選択された検証物体があると予測される方向にカメラ部１００を回転させる工程である。

前記最優先順位候補ノードの最優先順位物体を眺める工程Ｓ５４５は、位置推定部はパン／チルト装置１１０を用いてカメラを水平に左右回転するか、上下にうなずく動作を行うことができる。

前記その方向の環境イメージ及びステレオ深さ情報獲得工程Ｓ５５０は、カメラ部１００が向かっている方向の環境映像及びそれに対してステレオビジョンの基盤の下で距離情報を獲得する工程である。

前記物体認識工程Ｓ５５５は、位置演算部が前記その方向の環境イメージ及びステレオ深さ情報獲得工程Ｓ５５０で獲得した環境映像から局所的特徴点を抽出し、ロボット本体２００のデータベース部に格納された最優先順位物体の物体モデルの局所特徴点と比較して物体を認識する工程である。

前記認識された物体が最優先順位物体と一致するかを確認する工程Ｓ５６０は、位置演算部が前記物体認識工程Ｓ５５５で認識された物体と前記候補ノードに含まれている物体のロボットに対する位置計算及び優先順位計算工程Ｓ５４０で選択された検証物体を比較して一致すれば、その候補ノードを最終ノードとして選定する工程である。

次の優先順位候補ノードの最優先順位物体を眺める工程Ｓ５６２は、位置演算部が前記認識された物体が最優先順位物体と一致するかを確認する工程Ｓ５６０で認識された物体と最優先順位物体が一致しなければ、前記候補ノードの優先順位計算工程Ｓ５３５で現在カメラ部１００が向かっているノードの次の順位の優先順位を受けた候補ノードの前記候補ノードに含まれている物体のロボットに対する位置計算及び優先順位計算工程Ｓ５４０で計算された検証物体にカメラ部１００を移動させる工程である。

即ち、例えば、候補ノードがＡ、Ｂの２つであり、優先順位がＡ、Ｂである場合に、最優先順位ノードはＡとなる。既に認識された物体を除いたノードＡに属する物体がａ１、ｂ１、ｃ１であり、物体の優先順位がａ１、ｂ１、ｃ１であれば、ロボットはａ１があると予測される方向に顔を向け、物体認識を行う。もし、物体ａ１が認識されない場合、その次の優先順位である候補Ｂに属する最優先順位物体を見るように顔を向け、物体認識を行うようにする。

前記最終ノードにサンプリングしてパーティクル・フィルタリング・アルゴリズムを行う工程Ｓ５６５は、位置演算部が前記認識された物体が最優先順位物体と一致するかを確認する工程Ｓ５６０で選定された最終ノードに図８に示すパーティクル・フィルタリング・アルゴリズムのうち、サンプリング工程Ｓ２１０と、誤差計算工程Ｓ２２０と、重み値計算工程Ｓ２３０と、繰り返し条件確認工程Ｓ２４０を行い、正確な位置を推定するのに必要な最終重み値を計算する工程である。

前記最終ノードに対するロボットの２次元位置確定工程Ｓ５７０は、前記認識された順位が最優先順位物体と一致するかを確認する工程Ｓ５６０で選択され、前記最終ノードにサンプリングしてパーティクル・フィルタリング・アルゴリズムを行う工程Ｓ５６５で最終重み値が計算されたノードに前記式８を行ってロボットの位置を推定する工程である。

以上、添付の図面を参照して本発明の実施形態を説明したが、本発明の属する技術分野において通常の知識を有する者は、本発明がその技術的思想や必須な特徴を変更することなく、他の具体的な形態に実施できるということが理解できる。従って、以上で記述した実施形態は全ての面で例示的なものであり、限定されるものではない。

本発明による物体認識及び周辺環境情報に基づいて自己位置を推定するロボットの構造図である。 本発明による物体認識及び認識された物体を含む周辺環境情報に基づいたロボットの自己位置推定方法を示す基本順序図である。 本発明によるロボットが認識した物体と認識された物体を含む周辺環境概念図である。 本発明によるロボット中心の物体認識及び物体位置、認識された物体を含む周辺環境位置概念図である。 本発明による環境中心の物体位置及びその物体を含む周辺環境位置概念図である。 本発明によるロボット中心の物体及び認識された物体を含む周辺環境位置と環境中心の物体及びその物体を含む周辺環境位置間の関係図である。 本発明による物体認識及び認識された物体を含む周辺環境情報に基づいたロボットの自己位置推定方法で用いられる位相学的地図を示す例示図である。 パーティクルフィルタリングに基づいた位置推定方法を示す順序図である。 本発明の第１実施形態による物体認識及び認識された物体を含む周辺環境情報に基づいたロボットの自己位置推定方法を示す順序図である。 本発明の第２実施形態による物体認識及び認識された物体を含む周辺環境情報に基づいたロボットの自己位置推定方法を示す順序図である。 本発明の第３実施形態による物体認識及び認識された物体を含む周辺環境情報に基づいたロボットの自己位置推定方法を示す順序図である。 本発明の第３実施形態による物体認識及び認識された物体を含む周辺環境情報に基づいたロボットの自己位置推定方法を示す順序図である。

Claims (12)

1. ステレオ映像装置で形成されたカメラ部と、ロボット移動経路周囲の地図、該地図上の個別物体の局所特徴点及び該個別物体を含む周辺環境の局所特徴点が格納されたデータベース部と、前記カメラ部で獲得した映像情報を用いて自身の位置を推定する位置演算部とを有するロボットの自己位置推定方法であって、
   前記カメラ部でロボット周囲の映像を獲得する第１工程と、
   前記位置演算部で前記カメラ部が獲得した映像から局所特徴点を抽出して前記データベース部に格納された局所特徴点と比較することで前記カメラ部が獲得した映像内の個別物体を認識し、該個別物体の局所特徴点のカメラ座標系位置値と該個別物体を含む周辺環境の局所特徴点のカメラ座標系位置値を生成する第２工程と、
   前記位置演算部で前記データベース部に格納されている地図及び前記第２工程の結果に基づいて前記ロボットの位置を推定する第３工程とを含み、
   前記第３工程は、
   前記データベースに格納された地図から前記第２工程で認識した物体が１つ以上含まれているノードを前記ロボットが位置し得る候補ノードとして選定する候補ノード選定工程と、
   前記第２工程で生成された前記位置値を用いて選定された全ての前記候補ノードに対する前記ロボットの暫定位置を計算する暫定位置計算工程と、
   計算された前記暫定位置を初期位置値として選定された全ての前記候補ノードにパーティクルフィルタリング技法を適用して前記ロボットの位置を推定する位置推定工程とを含んでなることを特徴とするロボットの自己位置推定方法。
2. 前記暫定位置計算工程では下記の式１と式２を用いて前記ロボットの前記暫定位置を計算し、
   前記位置推定工程では最終重み値を計算してパーティクルフィルタリング技法に適用し、サンプルが最も多く収斂されたノードのサンプルのみで下記の式８を行って前記ロボットの位置を推定することを特徴とする請求項１に記載のロボットの自己位置推定方法。
   

   
3. 前記第３工程は、
   前記データベースに格納された地図から前記第２工程で認識した物体が１つ以上含まれているノードを前記ロボットが位置し得る候補ノードとして選定する候補ノード選定工程と、
   前記第２工程で生成された位置値を用いて選定された全ての前記候補ノードに対する前記ロボットの暫定位置を計算する暫定位置計算工程と、
   前記候補ノードの中から最終候補ノードを選択する最終候補ノード選択工程と、
   前記最終候補ノードに対する前記ロボットの暫定位置を初期位置値として前記最終候補ノードにパーティクルフィルタリング技法を適用して前記ロボットの位置を推定する位置推定工程と
   を含んでなることを特徴とする請求項１に記載のロボットの自己位置推定方法。
4. 前記暫定位置計算工程では下記の式１と式２を用いて前記ロボットの前記暫定位置を計算し、
   前記位置推定工程では最終重み値を計算してパーティクルフィルタリング技法に適用し、下記の式８を行って前記ロボットの位置を推定することを特徴とする請求項３に記載のロボットの自己位置推定方法。
   

   
5. 前記最終候補ノードは、前記候補ノードに対して下記の式１２により求められたノード近接重み値のうち最も大きな値を有するノードであることを特徴とする請求項３に記載のロボットの自己位置推定方法。
   

   
6. 

   （ここで、π^（ｎ）は最終重み値であり、ｎ_Ｎは候補ノードの個数である。）
7. 
8. 
9. 
10. 
11. ３６０°より小さな視野角を有し、ステレオ映像装置で構成されたカメラ部と、前記カメラ部を一定角度ずつ回転させることができる回転装置と、ロボット移動経路周囲の地図、該地図上の個別物体の局所特徴点及び該個別物体を含む周辺環境の局所特徴点を格納しているデータベース部と、前記カメラ部で獲得した映像情報を用いて自身の位置を推定する位置演算部とを有するロボットの自己位置推定方法であって、
    前記カメラ部が一定方向のロボット周囲の映像を獲得する第１工程と、
    前記位置演算部において前記カメラ部が獲得した映像内で認識した物体がない場合には前記回転装置を駆動してカメラを回転させた後、他の方向の映像を獲得する過程を、獲得された映像で局所特徴点を抽出して前記データベース部に格納された局所特徴と比較することで認識した個別物体が存在するまで繰り返し、認識した個別物体の局所特徴点のカメラ座標系位置値と該個別物体を含む周辺環境の局所特徴点のカメラ座標系位置値を生成する第２工程と、
    前記位置演算部で前記データベース部に格納された地図から前記認識された物体が１つ以上含まれているノードを前記ロボットが位置し得る候補ノードとして選定する第３工程と、
    前記位置演算部で前記候補ノードの優先順位を決定する第４工程と、
    前記位置演算部で前記候補ノードに含まれている物体と前記ロボット間の距離及び最優先順位物体を計算する第５工程と、
    前記位置演算部で前記回転装置を用いてカメラ部を回転させて前記候補ノードの最優先順位物体を確認する第６工程と、
    前記位置演算部で前記第６工程で前記最優先物体が確認されたノードにパーティクルフィルタリング技法を用いて前記ロボットの位置を推定する第７工程と
    を含むロボットの自己位置推定方法。
12. 前記第６工程は、前記候補ノードの最優先物体が認識できなければ、前期回転装置で前記カメラ部を回転させて前記第４工程での該当候補ノードのうち、次の優先順位の候補ノードの最優先物体を確認することを特徴とする請求項１１に記載のロボットの自己位置推定方法。

Images