(57)【要約】【課題】移動する虞のあるランドマークを用いた場合でも、自己位置を推定することが可能な自己位置推定装置を提供すること。【解決手段】ランドマークを配置情報データと照合する照合手段７と、照合できたランドマークを用いて自己位置を推定する自己位置推定手段８とを備え、自己位置推定手段８は、自己位置を推定する際に、信頼度の低いランドマークより信頼度の高いランドマークの影響度を大きくする。信頼度の低いランドマークの影響を小さくして、位置推定を行うことで、移動する虞のある対象物をランドマークとして用いることができる。そのため、ランドマークとして固定物を予め設置する必要がなくなる。【選択図】図１
2008040677.tif 000002
【特許請求の範囲】
【請求項１】
ランドマーク配置情報データを参照して自己位置を推定する自己位置推定装置であって、
ランドマークを前記ランドマーク配置情報データと照合する照合手段と、
前記照合手段によって照合できたランドマークを用いて自己位置を推定する自己位置推定手段とを備え、
前記自己位置推定手段は、信頼度の低いランドマークより信頼度の高いランドマークの影響度を大きくして、自己位置を推定することを特徴とする自己位置推定装置。
【請求項２】
ランドマークの信頼度は、移動する可能性が高いランドマークより、移動する可能性が低いランドマークのほうが、高く評価されることを特徴とする請求項１記載の自己位置推定装置。
【請求項３】
ランドマークを検出する検出手段を更に備え、
ランドマークの信頼度は、前記検出手段によって検出し難いランドマークより、前記検出手段によって検出し易いランドマークのほうが、高く評価されることを特徴とする請求項１又は２記載の自己位置推定装置。
【請求項４】
位置検出精度が異なる複数種類の前記検出手段を備え、
ランドマークの信頼度は、位置検出精度の低い前記検出手段によって検出されたランドマークより、位置検出精度の高い前記検出手段によって検出されたランドマークのほうが、高く評価されることを特徴とする請求項１〜３の何れか一項に記載の自己位置推定装置。
【請求項５】
ランドマークの信頼度は、移動したランドマークより、移動しなかったランドマークのほうが、高く評価されることを特徴とする請求項１〜４の何れか一項に記載の自己位置推定装置。
【請求項６】
ランドマークの信頼度は、ランドマークの属する種類に応じて評価されることを特徴とする請求項１〜５の何れか一項に記載の自己位置推定装置。
【請求項７】
ランドマークの信頼度は、ランドマークが存在する場所に応じて評価されることを特徴とする請求項１〜６の何れか一項に記載の自己位置推定装置。
【請求項８】
ランドマークの信頼度を評価する評価手段を更に備えることを特徴とする請求項１〜７の何れか一項に記載の自己位置推定装置。
【請求項９】
ランドマークの信頼度に関する情報を予め記憶した信頼度情報記憶手段を備えたことを特徴とする請求項１〜８の何れか一項に記載の自己位置推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自己位置を推定する自己位置推定装置に関する。
【背景技術】
【０００２】
従来、走行路を移動する移動体に搭載されて当該移動体の自己位置を推定する技術として、下記の特許文献１及び特許文献２に示すようなものがある。特許文献１に記載の移動体の自己位置同定装置は、走行路（道路）脇に設置された標識、信号機、電柱等をランドマークとし、ランドマークを撮像する撮像手段と、実際のランドマークの位置及び種類の情報が予め記述された地図データを保持する地図データ記憶手段とを備えている。この移動体の自己位置同定装置では、撮像手段によって撮像された画像からランドマーク像を抽出してランドマークの位置及び種類を認識し、この認識されたランドマークの位置及び種類と、地図データ記憶手段に記憶された地図データとを照合して、移動体の自己位置を同定している。
【０００３】
また、特許文献２に記載の無人車の誘導システムでは、予め配置されたランドマークを一対のカメラによって撮影し、撮影されたランドマークに対する無人車（移動体）の位置を求めると共に、撮影されたランドマークを参照パターンと照合することにより絶対座標系でのランドマークの位置を定めて、無人車の位置を定めている。この無人車の誘導システムでは、複数のランドマークを識別するために、各ランドマークに固有のパターンを設けている。
【特許文献１】特開平８−２４７７７５号公報
【特許文献２】特開平９−２１８７１２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、上記特許文献１に記載の従来技術では、道路標識等の固定物をランドマークとして移動体の自己位置を定めているため、道路標識のような共通する固定物が少ない環境（例えば、屋内）において、移動体の自己位置を推定することは困難であった。
【０００５】
また、上記特許文献２に記載の従来技術では、固有のパターンを設けたランドマークに基づいて、移動体の自己位置を定めているため、予め所定の位置にランドマークを配置する必要があった。また、正確な位置を求めるためには、多くのランドマークを設置する必要があった。しかし、人間が生活する実環境（例えば、室内）において、固定的ランドマークを数多く設置することは困難であった。
【０００６】
本発明は、このような課題を解決するために成されたものであり、移動する虞のあるランドマークを用いた場合でも、自己位置を推定することが可能な自己位置推定装置を提供することを目的とする。
【課題を解決するための手段】
【０００７】
本発明による自己位置推定装置は、ランドマーク配置情報データを参照して自己位置を推定する自己位置推定装置であって、ランドマークを前記ランドマーク配置情報データと照合する照合手段と、照合手段によって照合できたランドマークを用いて自己位置を推定する自己位置推定手段とを備え、自己位置推定手段は、信頼度の低いランドマークより信頼度の高いランドマークの影響度を大きくして、自己位置を推定することを特徴としている。
【０００８】
このような自己位置推定装置では、ランドマークをランドマーク配置情報データと照合する照合手段と、照合できたランドマークを用いて自己位置を推定する自己位置推定手段とを備えている。自己位置推定手段は、自己位置を推定する際に、信頼度の低いランドマークより信頼度の高いランドマークの影響度を大きくする。これにより、信頼度の低いランドマークの影響を小さくして、位置推定を行うので、移動する虞のある対象物でも、信頼度を低くしてランドマークとして用いることができる。その結果、ランドマークとして固定物を予め設置する必要がなく、ランドマークとしての固定物が少ない環境であっても、自己位置を推定することが可能となる。特に、環境が変化し易い生活環境等において、自己位置を推定する際に有効である。また、信頼度の高いランドマークの影響度を大きくして自己位置推定を行うので、位置精度の向上を図ることができる。また、従前の技術では、移動した物体を検出した場合、移動した物体をランドマークとして採用せずに除外していた。そのため、位置推定に用いるランドマークが減ってしまい、位置精度が低下する虞があった。しかし、移動した対象物であっても、信頼度を低くしてランドマークとして用いることで、位置推定に用いるランドマークの数を十分に確保することができる。その結果、位置精度の低下を防止することができる。なお、ここでいう「ランドマーク」とは、位置推定において目印になるものであり、例えば、実環境に存在する物体自体、物体の任意の点を含むものである。この「ランドマーク」は、移動可能な物体や移動可能な物体の任意の点でもよく、固定物や固定物の任意の点でもよい。また、ここでいう「ランドマークの信頼度」とは、自己位置を推定する際に、有効なランドマークであるかを示す指標となるものである。
【０００９】
ここで、ランドマークの信頼度は、移動する可能性が高いランドマークより、移動する可能性が低いランドマークのほうが、高く評価されることが好ましい。これにより、移動する可能性の低いランドマークの影響を大きくし移動する可能性の高いランドマークの影響を小さくして自己位置の推定を行うので、位置精度の向上を図ることができる。
【００１０】
また、ランドマークを検出する検出手段を更に備え、ランドマークの信頼度は、検出手段によって検出し難いランドマークより、検出手段によって検出し易いランドマークのほうが、高く評価されることが好ましい。検出手段の特性、ランドマークの色、柄、形状等により、同じランドマークを安定して検出できる場合と安定して検出できない場合がある。安定して検出できるランドマークの影響を大きくして安定して検出できないランドマークの影響を小さくして自己位置を推定するので、位置精度の向上を図ることができる。
【００１１】
また、位置検出精度が異なる複数種類の検出手段を備え、ランドマークの信頼度は、位置検出精度の低い検出手段によって検出されたランドマークより、位置検出精度の高い検出手段によって検出されたランドマークのほうが、高く評価されることが好ましい。例えばカメラ、レーザレンジファインダ、ＲＦＩＤタグリーダ等の複数種類の検出手段を併用する場合、検出手段の種類によってランドマークの位置検出精度が異なる。位置検出精度の高い検出手段によって検出されたランドマークの影響を大きくして位置検出精度の低い検出手段によって検出されたランドマークの影響を小さくして自己位置を推定するので、位置精度の向上を図ることができる。
【００１２】
また、ランドマークの信頼度は、移動したランドマークより、移動しなかったランドマークのほうが、高く評価されることが好ましい。これにより、ランドマークが移動した実績に応じて信頼度を変化させることができる。また、移動しなかったランドマークの影響を大きくし移動したランドマークの影響を小さくして自己位置を推定することができるので、位置精度の向上を図ることができる。
【００１３】
また、ランドマークの信頼度は、ランドマークの属する種類に応じて評価されることが好ましい。これにより、初めて検出されたランドマークについて、ランドマークの属する種類に応じて、同じ種類のランドマークの信頼度を適用することができる。例えば、検出された対象物がコップである場合、過去に検出された別のコップの信頼度を引き継ぐことができる。
【００１４】
また、ランドマークの信頼度は、ランドマークが存在する場所に応じて評価されることが好ましい。例えば、特定の領域内（場所）に存在する複数のランドマークが、頻繁に移動している場合、この領域内のランドマークは、移動し易いものと判定することができるため、ランドマークの信頼度を低くする。これにより、この領域内に存在するランドマークの影響を小さくして自己位置を推定することができる。その結果、位置精度の向上を図ることができる。また、特定の領域内に存在する複数のランドマークが、一定時間動かない場合、この領域内のランドマークは、移動し難いものと判定することができるため、ランドマーク情報の信頼度を高くする。これにより、この領域内に存在するランドマークの影響を大きくして自己位置を推定することができる。その結果、位置精度の向上を図ることができる。
【００１５】
また、ランドマークの信頼度を評価する評価手段を更に備えることが好ましい。これにより、実環境の変化に合わせて、ランドマークの信頼度を評価することができるので、実環境の変化に対応したランドマークを用いて、自己位置を推定することができ、位置精度の向上を図ることができる。
【００１６】
また、ランドマークの信頼度に関する情報を予め記憶した信頼度情報記憶手段を備えることが好ましい。予め記憶されたランドマークの信頼度に関する情報を用いることで、位置推定の際の処理速度を向上させることができる。
【発明の効果】
【００１７】
本発明の自己位置推定装置によれば、固定の対象物だけでなく移動する可能性のある対象物もランドマークとして利用して、自己位置を推定することができると共に、位置推定精度の向上を図ることができる。
【発明を実施するための最良の形態】
【００１８】
以下、本発明による自己位置推定装置の好適な実施形態について、図面を参照しながら説明する。なお、図面の説明において、同一または相当要素には同一の符号を付し、重複する説明は省略する。
【００１９】
本発明の自己位置推定装置は、例えば人間の生活環境中で移動可能なロボット等の移動体に搭載され、移動体の自己位置を推定するものである。この自己位置推定装置は、対象物を検出し、検出された対象物をランドマークとして、ランドマークと地図データ（ランドマーク配置情報データ）とを照合し、照合できたランドマークに基づいて、移動体の自己位置を推定する。地図データは、ランドマークの絶対的な位置及び特徴に関する情報を含むものであり、ランドマークの絶対的な位置とは、任意の点（例えば、部屋の角）を基準（０，０，０）とした位置である。第１実施形態として、地図作成機能、地図更新機能（詳しくは後述する）を備えない自己位置推定装置、第２実施形態として、地図作成機能、地図更新機能を備える自己位置推定装置について説明する。なお、ここでいう「ランドマーク」とは、位置推定において目印になるものであり、実環境に存在する物体、この物体の任意の点を含むものである。この「ランドマーク」は、移動可能な物体及び移動可能な物体の任意の点でもよく、固定物及び固定物の任意の点でもよい。
【００２０】
まず、第１実施形態の自己位置推定装置について、図１〜図６を参照しながら説明する。図１は、本発明の第１実施形態に係る自己位置推定装置を示すシステム構成図である。図１に示す自己位置推定装置１は、ランドマークとして用いられる対象物を検出する複数のランドマーク検出センサ２と、位置推定処理を実行する位置推定用電子制御ユニット（以下、位置推定用ＥＣＵという）３とを備えている。
【００２１】
ランドマーク検出センサ２は、実環境からランドマークを抽出するためのデータ（例えば、画像データ）を検出するものである。自己位置推定装置１は、複数のランドマーク検出センサ２として、ステレオカメラ（以下、カメラという）、レーザレンジファインダ、ＲＦＩＤタグリーダ等を備えている。本発明の検出手段は、ランドマーク検出センサ２及び後述するランドマーク抽出部６によって構成され、ランドマーク検出センサ２によって、「ランドマークを抽出するためのデータ」を検出し、ランドマーク抽出部６によって、ランドマークを抽出する。
【００２２】
ランドマーク検出センサ２によって検出される「ランドマークを抽出するためのデータ」とは、ランドマークの特徴に関する情報（特徴量）、ランドマークの位置（ランドマークと移動体との相対的な位置）に関する情報である。なお、ここでいう「ランドマークの特徴に関する情報（特徴量）」とは、ランドマークの種別を識別可能な情報である。例えば、カメラによって画像を撮像した場合、対象物の輪郭や絵柄等が、「ランドマークの特徴に関する情報」となる。この輪郭や絵柄等に基づいてランドマークの種別を識別する。
【００２３】
位置推定用ＥＣＵ３は、演算処理を行うＣＰＵ４、記憶部５となるＲＯＭ及びＲＡＭ、入力信号回路、出力信号回路、電源回路などを備え、上記複数のランドマーク検出センサ２と電気的に接続されている。この位置推定用ＥＣＵ３では、記憶部５に記憶された地図データを参照して移動体の位置推定を行う。
【００２４】
位置推定用ＥＣＵ３の記憶部５には、ＣＰＵ４を作動させるためのプログラムなどが記憶されている。また、この位置推定用ＥＣＵ３の記憶部５は、ランドマークに関する情報を含む地図データを予め記憶している。また、位置推定用ＥＣＵ３の記憶部５は、本発明の信頼度情報記憶手段として機能するものであり、ランドマークの評価値（ランドマーク情報の信頼度）を予め記憶している。
【００２５】
次に、ランドマークの評価値について説明する。ここでいう「ランドマークの評価値」とは、位置を推定する際に、有効なランドマークであるかを示す指標となるものであり、例えば、移動しにくいランドマークが信頼度の高いランドマークとなる。本実施形態では、「ランドマークの評価基準」に基づいてランドマークを評価して、評価値のランクを０．０〜１．０に分類する。最も信頼度の高いランドマークの評価値を「１．０」とし、最も信頼度の低いランドマークの評価値を「０．０」としている。自己位置推定装置１では、複数の評価基準、（「外的要因による移動されやすさ」、「センサ特性による安定度」）に応じて、ランドマークの評価値を設定している。
【００２６】
次に、「外的要因による移動されやすさ」に基づくランドマークの評価について、説明する。「外的要因による移動されやすさ」に基づくランドマークの評価では、ランドマークが人為的に動かされ易いか否かを評価基準とし、より詳細に、「ランドマークの高さによる評価」、「ランドマークの場所による評価」、「ランドマークの空間的な配置による評価」に応じて、ランドマークの評価値を設定している。
【００２７】
「ランドマークの高さによる評価」では、ランドマークが検出された高さに応じて、評価値Ｗ１を設定する。高い場所に存在するものは、人間の手が届き難いためランドマークが移動する可能性が低く、低い場所に存在するものは、人間の手が届き難いためランドマークが移動する可能性が高くなる。従って、ランドマークの検出位置が高い場合には、ランドマークの評価値Ｗ１を高く設定し、ランドマークの検出位置が低い場合には、ランドマークの評価値Ｗ１を低く設定する。
【００２８】
図２は、屋内環境を示す概略図であり、「ランドマークの高さによる評価」に基く評価値を設定するランドマークの一例を示すものである。図２に示す屋内環境では、床面１１上に棚１２、ロッカー１３が配置されている。ここでは、床面１１上のランドマーク１４、棚１２に設置されたランドマーク１５、ロッカー１３の外壁に設置されたランドマーク１６が検出されている。「ランドマークの高さによる評価」に基づいて設定された評価値Ｗ１として、床面１１上のランドマーク１４の評価値Ｗ１は「０．１」、棚１２に設置されたランドマーク１５の評価値Ｗ１は「０．４」、ロッカーの外壁に設置されたランドマーク１６の評価値Ｗ１は「０．９」となる。これにより、高い場所にあるランドマークの影響を大きくして、低い場所にあるランドマークの影響を小さくして、位置推定を行うことができる。
【００２９】
「ランドマークの場所による評価」では、ランドマークが検出された場所に応じて、評価値Ｗ２を設定する。すなわち、任意の領域内のランドマークの移動し易さに応じ、場所（領域）ごとに評価値Ｗ２を設定する。例えば、良く使う棚の中に存在するものは、移動され易いため、棚の中に存在するランドマークの評価値Ｗ２を低く設定し、ほとんど使わない棚の中に存在するものは、移動し難いため、ランドマークの評価値Ｗ２を高く設定する。また、床面に存在するものは、人為的に移動させやすいため、ランドマークの評価値Ｗ２を低く設定する。
【００３０】
図３は、屋内環境を示す概略図であり、「ランドマークの場所による評価」に基く評価値を設定するランドマークの一例を示すものである。図３に示すように、床面１１上のランドマーク１７、棚１２の中段１２ａに設置されたランドマーク１８、棚１２の上段１２ｂに設置されたランドマーク１９が検出されている。棚１２の中段１２ａは、棚１２の上段１２ｂよりも利用頻度が低い場所領域である。「ランドマークの場所による評価」に基づいて設定された評価値Ｗ２として、床面１１上のランドマーク１７の評価値Ｗ２は「０．１」、棚１２の中段１２ａに設置されたランドマーク１８の評価値Ｗ２は「０．８」、棚１２の上段１２ｂに設置されたランドマーク１９の評価値Ｗ２は「０．４」となる。これにより、ランドマークが検出された場所の環境変化の程度に応じて、評価値Ｗ２を変化させることができる。
【００３１】
「ランドマークの空間的な配置による評価」では、ランドマークが検出された空間的な配置に応じて、評価値Ｗ３を設定する。この場合、ランドマーク検出センサ２として３次元空間復元センサ（例えばステレオカメラ、レーザレンジファインダ）によって検出されたデータに基づいて、ランドマークの空間的な配置を計測することが好ましい。鉛直面に接しているものは、鉛直面に固定されている場合が多いためランドマークが移動する可能性が低く、水平面に接しているものは、水平面（例えば床面、机の上面）上にそのまま配置される場合が多いためランドマークが移動する可能性が高くなる。従って、ランドマークが鉛直面に接している場合には、ランドマークの評価値Ｗ３を高く設定し、ランドマークが水平面に接している場合には、ランドマークの評価値Ｗ３を低く設定する。
【００３２】
図４は、屋内環境を示す概略図であり、「ランドマークの空間的な配置による評価」に基く評価値を設定するランドマークの一例を示すものである。図４に示すように、床面１１（水平面）上のランドマーク２０、棚１２の天板１２ｃ（水平面）上に存在するランドマーク２１、ロッカー１３の外壁１３ａ（鉛直面）に設置されたランドマーク２２が検出されている。「ランドマークの空間的な配置による評価」に基づいて設定された評価値Ｗ３として、床面１１上のランドマーク２０の評価値Ｗ３は「０．２」、棚１２の天板１２ｃ上に存在するランドマーク２１の評価値Ｗ３は「０．２」、ロッカー１３の外壁１３ａに設置されたランドマーク２２の評価値Ｗ３は「０．８」となる。これにより、ランドマークの空間的な配置に基づいて、評価値Ｗ３を変化させることができる。
【００３３】
次に、「センサ特性による安定度」に基づくランドマークの評価について、説明する。「センサ特性による安定度」に基づくランドマークの評価では、ランドマーク検出センサ２のセンサ特性により、複数回検出しても、安定して同じランドマークを検出できるか否かを評価基準とし、より詳細に、「ランドマークの特性による評価」、「ランドマーク検出センサによる評価」に応じて、ランドマークの評価値を設定している。
【００３４】
「ランドマークの特性による評価」では、ランドマーク検出センサ２による検出され易さ（検出安定度）に応じて、評価値Ｗ４を設定する。例えば、カメラによって取得された画像中のコーナー部は、コーナー部とその周囲とのコントラストによって、ランドマークとして検出される場合と検出されない場合がある。コントラストが高くはっきりしたエッジは、安定して検出され、コントラストが低くぼやけたエッジは、安定して検出されない場合がある。また、ランドマークの色、形状、柄等により、ランドマーク検出センサ２によって、安定して検出される場合と、安定して検出されない場合がある。従って、ランドマーク検出センサ２によって、安定して検出されるランドマークは、その評価値Ｗ４を高く設定し、安定して検出することができないランドマークは、その評価値Ｗ４を低く設定する。
【００３５】
「ランドマーク検出センサによる評価」では、ランドマーク検出センサ２によるランドマークの位置検出精度に応じて、評価値Ｗ５を設定する。複数種類のランドマーク検出センサ２を備えている場合、ランドマーク検出センサ２によって位置検出精度が異なる。そこで、位置検出精度が高いランドマーク検出センサ２によって検出されたランドマークの評価値Ｗ５を高く設定し、位置検出精度が低いランドマーク検出センサ２によって検出されたランドマークの評価値Ｗ５を低く設定する。
【００３６】
これらの評価基準によって設定された複数の評価値は、ランドマークごとに、下記式（１）によって平均値を算出し、この評価値の平均（最終評価値Ｗｆ）を用いて、位置推定を行う。
【数１】
2008040677.tif 000003
（ただし、Ｗｆ：最終評価値（０．０〜１．０）、Ｗα：各評価基準での評価値、Ａα：各評価基準の重み、Ｎ：評価基準数）
なお、「各評価基準の重みＡα」は、実験データや過去の実績等によって求められる数値であり、評価基準の重要度（優先度）に応じて、設定される。
【００３７】
そして、ランドマークの絶対的な位置（ｘ，ｙ，ｚ）、特徴量、評価値Ｗ１〜Ｗ５，Ｗｆが、地図データとして記憶部５に記憶されている。
【００３８】
位置推定用ＥＣＵ３のＣＰＵ４は、記憶部５に記憶されたプログラムを実行することで、ランドマーク抽出部６、地図照合部（照合手段）７、位置推定部（自己位置推定手段）８として機能する。
【００３９】
ランドマーク抽出部６では、ランドマーク検出センサ２によって取得された各データ（「ランドマークを抽出するためのデータ」）からランドマークを抽出する。抽出されるランドマークとしては、対象物の角、エッジ、特徴的な形状や絵柄である。図５は、カメラによって撮像された画像の一例を示す概略図である。図５に示すように、例えば、カメラによって撮像された画像データに基づいてランドマークが抽出される。ランドマークとしては、例えば、棚のコーナー部、本のコーナー部等が抽出される。
【００４０】
地図照合部７では、ランドマーク抽出部６によって抽出されたランドマークを記憶部５に記憶された地図データと照合する。図６は、記憶部に記憶された地図データ（ランドマークの位置）の一例を示す概略図である。具体的には、抽出された各ランドマークの特徴量と、地図データの各ランドマークの特徴量とをそれぞれ比較し、特徴量が一致するものを探す。図５及び図６に示す例の場合、図５に示すランドマーク２３，２４，２５，２６と、図６に示すランドマーク２７，２８，２９，３０とが対応している。
【００４１】
位置推定部８では、地図照合部７で照合できたランドマークに関する情報（検出されたデータ、記憶部５に記憶されている地図データ）に基づいて、移動体の自己位置を推定する位置推定処理を行う。評価値が閾値以上であるランドマークが充分に抽出された場合、評価値が閾値以上であるランドマークに基づいて位置推定を行う。一方、評価値が閾値以上であるランドマークが充分に抽出されなかった場合、評価値の高いランドマークの影響度を大きくして位置推定を行う。
【００４２】
例えば、カメラによって撮像された画像データ、ランドマークの３次元位置情報を用いて位置推定を行う場合、下記式（２）「重み付き評価関数Ｐ」を利用する。下記式（２）を利用して、重み付き評価関数Ｐが最小となるように、回転成分Ｒ（ロール、ヨー、ピッチ）、平行移動成分ｔ（Ｘ，Ｙ，Ｚ）を求めることで、移動体の位置推定を行う。
【数２】
2008040677.tif 000004
（ただし、β：ランドマークナンバー、Ｗｆβ：最終評価値（０．０〜１．０）、ｍβ：検出したランドマークの位置（ｘ，ｙ）、Ｍβ：地図データとして記憶されたランドマークの３次元位置（Ｘ，Ｙ，Ｚ）、Ｋ：カメラの内部パラメータ（カメラをキャリブレーションしておくことで既知となる定数）、Ｒ：回転成分（ロール、ヨー、ピッチ）、ｔ：平行移動成分（Ｘ，Ｙ，Ｚ））
この数式（２）を用いて、位置推定を行うことで、位置推定精度の向上を図ることができる。また、ランドマークとしての固定物を設置していない環境であっても、位置推定を行うことができる。
【００４３】
次に、第１実施形態に係る自己位置推定装置１の基本動作について図面を参照して説明する。各ランドマーク検出センサ２は、移動体の周囲の情報を収集し、位置推定用ＥＣＵ３に転送する。例えば、カメラによって、移動体の周囲を撮像して、画像データを位置推定用ＥＣＵ３に転送する。
【００４４】
次に、位置推定用ＥＣＵ３で実行される位置推定処理について図７のフローチャートに沿って説明する。図７は、位置推定用ＥＣＵで実行される位置推定処理の動作手順を示すフローチャートである。位置推定用ＥＣＵ３では、まず、各ランドマーク検出センサ２から検出データを入力する（Ｓ１）。次に、位置推定用ＥＣＵ３では、ランドマーク抽出処理を行い、ランドマークの抽出を行う（Ｓ２）。
【００４５】
位置推定用ＥＣＵ３は、ランドマークを抽出した後、地図照合処理を行う（Ｓ３）。次に、照合できたランドマーク情報を用いて、移動体の自己位置推定を行う（Ｓ４）。ここでは、ランドマーク毎に評価値Ｗｆに応じ位置推定に対する影響度を変化させて自己位置推定を行う。位置推定用ＥＣＵ３は、移動体の自己位置を推定した後、結果を例えば移動体を制御する制御部等に出力し（Ｓ５）位置推定処理を終了する。
【００４６】
このような自己位置推定装置１では、ランドマーク検出センサ２によって検出された対象物をランドマークとし、評価値の低いランドマークの影響を小さくして、位置推定を行うので、移動する虞のある対象物でも、ランドマークとして用いることができる。その結果、ランドマークとして固定物を予め設置する必要がなく、ランドマークとしての共通の固定物が少ない環境であっても、自己位置を推定することが可能となる。特に、環境が変化し易い生活環境等において、自己位置を推定する際に有効である。また、評価値の高いランドマークの影響度を大きくして自己位置推定を行うので、位置精度の向上を図ることができる。また、従前の技術では、移動した物体を検出した場合、移動した物体をランドマークとして採用せずに除外していた。そのため、位置推定に用いるランドマークが減ってしまい、位置精度が低下する虞があった。しかし、自己位置推定装置１では、移動した対象物であっても、信頼度を低くしてランドマークとして用いることができるので、位置推定に用いるランドマークの数を十分に確保することができる。その結果、位置精度の低下を防止することができる。
【００４７】
また、「ランドマークの高さによる評価」に基づく評価値Ｗ１は、ランドマークが検出された高さに応じて評価されている。ランドマークの検出位置が高い場合には、ランドマークが移動し難いと判断し、ランドマークの評価値Ｗ１が高く設定されている。ランドマークの検出位置が低い場合には、ランドマークが移動し易いと判断し、ランドマークの評価値Ｗ１が低く設定されている。これにより、移動する可能性の低いランドマークの影響を大きくし移動する可能性の高いランドマークの影響を小さくして自己位置の推定を行うことができ、位置精度の向上を図ることができる。
【００４８】
また、「ランドマークの場所による評価」に基づく評価値Ｗ２を用いて自己位置の推定を行うことで、移動する可能性の低いランドマークの影響を大きくし、移動する可能性の高いランドマークの影響を小さくすることができる。これにより、位置精度の向上を図ることができる。
【００４９】
また、「ランドマークの空間的な配置による評価」に基づく評価値Ｗ３を用いて自己位置の推定を行うことで、移動する可能性の低いランドマークの影響を大きくし、移動する可能性の高いランドマークの影響を小さくして自己位置の推定を行うことができる。これにより、位置精度の向上を図ることができる。
【００５０】
また、「ランドマークの特性による評価」に基づく評価値Ｗ４を用いて、自己位置の推定を行うことで、安定して検出できるランドマークの影響を大きくして安定して検出できないランドマークの影響を小さくして移動体の自己位置を推定することができる。これにより、位置精度の向上を図ることができる。
【００５１】
また、「ランドマーク検出センサによる評価」に基づく評価値Ｗ５を用いて、自己位置の推定を行うことで、位置検出精度の高いランドマーク検出センサ２によって検出されたランドマークの影響を大きくして位置検出精度の低いランドマーク検出センサ２によって検出されたランドマークの影響を小さくして移動体の自己位置を推定することができる。これにより、位置精度の向上を図ることができる。
【００５２】
また、自己位置推定装置１では、ランドマークの評価値Ｗ１〜Ｗ５を予め記憶した記憶部５を備えているため、位置推定用ＥＣＵ３における処理速度を向上させることができる。
【００５３】
次に、本発明の第２実施形態に係る自己位置推定装置について、図面を参照しながら説明する。図８は、本発明の第２実施形態に係る自己位置推定装置を示すシステム構成図である。この第２実施形態の自己位置推定装置４１が第１実施形態の自己位置推定装置１と違う点は、位置推定機能を有する位置推定用ＥＣＵ３に代えて、位置推定機能の他に地図作成機能、地図更新機能を有する位置推定用ＥＣＵ４３を備える点及び位置計測部４２を備える点である。
【００５４】
位置推定用ＥＣＵ４３では、第１実施形態と同様に自己位置を推定することができる。さらに、位置推定用ＥＣＵ４３では、地図作成及び地図更新を行うことができる。
【００５５】
また、自己位置推定装置４１は、地図作成時、地図更新時に、地図データとして登録するランドマークの位置を計測する位置計測部４２を備えている。この位置計測部４２は、ＧＰＳ、測量器具などにより構成される。計測されたランドマークの位置に関する情報は、地図作成部４８に入力されて、記憶部５に記憶される。なお、位置計測部４２を備えない構成とし、ランドマークの位置に関する情報を手入力するようにしてもよし、あるいはランドマーク検出センサ２を利用してもよい。
【００５６】
次に、地図作成機能について説明する。位置推定用ＥＣＵ４３のＣＰＵ４４は、地図作成時において、記憶部５に記憶されたプログラムを実行することで、ランドマーク抽出部６、評価値設定部（評価手段）４７、地図作成部４８として機能する。なお、地図作成は、自己位置推定装置１の起動時等において、初期データ作成するために実行するものである。
【００５７】
評価値設定部４７では、ランドマーク抽出部６によって抽出されたランドマークについて、評価値Ｗ１〜Ｗ５を設定する。この評価値設定部４７は、ランドマーク抽出部６で抽出した各ランドマークに対して、第１実施形態と同様の方法によって複数の評価基準「外的要因による移動されやすさ」、「センサ特性による安定度」に応じて、ランドマークの評価値Ｗ１〜Ｗ５を設定する。さらに、評価値設定部４７では、評価値Ｗ１〜Ｗ５に基づいて、最終評価値Ｗｆを算出する。
【００５８】
地図作成部４８では、ランドマーク毎に、評価値設定部４７で設定した評価値Ｗ１〜Ｗ５、最終評価値Ｗｆと抽出されたランドマークの位置及び特徴に関する情報を含むデータを地図データとして記憶部５に記憶させる。
【００５９】
次に、地図更新機能について説明する。位置推定用ＥＣＵ４３のＣＰＵ４４は、位置推定時において、ランドマークの移動を検知した場合及び新たなランドマークを検知した場合に、地図更新を行う。位置推定用ＥＣＵ４３のＣＰＵ４４は、地図更新時において、記憶部５に記憶されたプログラムを実行することで、評価値設定部４７、地図作成部４８として機能する。
【００６０】
地図照合部７でランドマークの移動を検知した場合、評価値設定部４７では、評価値Ｗ１〜Ｗ５及び最終評価値Ｗｆの再設定を行う。更に、評価値設定部４７では、「ランドマークの移動実績による評価」に応じてランドマークの評価値Ｗの再設定を行う。地図照合部７において、移動したと判定されたランドマークの評価値Ｗを下げる一方、一定時間移動しなかったと判定されたランドマークの評価値Ｗを上げる。
【００６１】
図９は、屋内環境を示す概略図であり、「ランドマークの移動実績による評価」に基く評価値を設定するランドマークの一例を示すものである。図９に示すように、例えば、ロッカー１３の外壁に存在するランドマーク５１が破線で示す位置Ａから実線で示す位置Ｂに移動したと判定された場合、ランドマーク５１の評価値を「１．０」から「０．８」へ下げる。
【００６２】
また、評価値設定部４７では、「ランドマークの場所による評価」で適用される領域ごとの評価値（領域補正値）Ｗ２を再設定する。領域内のランドマークが複数移動している場合には、同一の領域内に存在するランドマークの評価値Ｗ２を下げる一方、領域内のランドマークが一定時間移動しなかった場合には、同一の領域内に存在するランドマークの評価値Ｗ２を上げる。これにより、領域内のランドマークの移動実績が、同一の領域内の他のランドマークの評価値の設定に反映されることになる。
【００６３】
図１０は、屋内環境を示す概略図であり、「ランドマークの場所による評価」に基く評価値を設定するランドマークの一例を示すものである。図１０に示すように、棚１２の上段１２ｂ内（場所領域内）の複数のランドマーク５２〜５５が移動している場合、ランドマーク５２〜５５の評価値Ｗ２を「１．０」から「０．６」へ下げる。
【００６４】
また、新たなランドマークを検知した場合、評価値設定部４７では、評価値Ｗ１〜Ｗ５を設定する。そこで、評価値設定部４７では、「ランドマークの属する種類による評価」に応じてランドマークの評価値を設定する。具体的には、新たに検知されたランドマークの属する種類（同じ特徴量をもつか否か）を判定して、同一種類のランドマークの評価値Ｗ１〜Ｗ５を引き継ぐことができる。同じ種類の物体であれば、移動の有無、配置される場所等、同様の性質を示す可能性が高いため、評価値Ｗ１〜Ｗ５を引き継ぐことができる。ランドマークとして例えばコップが抽出された場合、コップが配置される場所等は、略限定される可能性が高く、他のコップの評価値Ｗ１〜Ｗ５を引き継ぐことで、ランドマークを更新する際の処理速度を速めることができる。
【００６５】
図１１は、ランドマークの評価値の一例を示す表である。ランドマーク（Ｌ１〜Ｌ４）ごとに、ランドマーク検出センサ２の種別、位置、特徴量、複数の評価値Ｗ１〜Ｗ３、検出回数等が記憶されている。なお、図１１では、評価値Ｗ１〜Ｗ３を記載し、評価値Ｗ４，Ｗ５の記載を省略している。例えば、新たなランドマークＬ４を検知した場合、記憶されているランドマークＬ１〜Ｌ３の特徴量Ｖ１〜Ｖ３と、新たなランドマークＬ４の特徴量Ｖ３とを比較し、特徴量Ｖ３が同じランドマークＬ３の評価値を引き継ぐことができる。
【００６６】
次に、第２実施形態に係る自己位置推定装置４１の基本動作について図面を参照して説明する。以下、位置推定用ＥＣＵ４３における制御処理について説明する。位置推定処理は、第１実施形態の自己位置推定装置１と同様であるため省略し、地図作成処理、地図更新処理について説明する。
【００６７】
位置推定用ＥＣＵ４３で実行される地図作成処理について図１２のフローチャートに沿って説明する。図１２は、位置推定用ＥＣＵで実行される地図作成処理の動作手順を示すフローチャートである。検出データ入力（Ｓ１）、ランドマーク抽出処理（Ｓ２）では、第１実施形態の位置推定処理と同じ処理を実行する。
【００６８】
位置推定用ＥＣＵ４３は、ランドマークを抽出した後、評価値設定処理を行う（Ｓ２３）。ここでは、「ランドマークの高さによる評価」、「ランドマークの場所による評価」、「ランドマークの空間的な配置による評価」、「ランドマークの特性による評価」、「ランドマーク検出センサによる評価」に応じて、ランドマークの評価値Ｗ１〜Ｗ５を設定する。
【００６９】
位置推定用ＥＣＵ４３は、ランドマークの評価値Ｗ１〜Ｗ５を設定した後、ランドマークのデータ（地図データ）を作成する（Ｓ２４）。記憶部５は、作成されたランドマークのデータを地図データとして記憶する（Ｓ２５）。
【００７０】
次に、位置推定用ＥＣＵ４３で実行される地図更新処理について図１３のフローチャートに沿って説明する。図１３は、位置推定用ＥＣＵで実行される地図更新処理の動作手順を示すフローチャートである。地図更新処理は、位置推定処理において、ランドマークの移動を検知した場合、及び新たなランドマークを検知した場合に、地図更新処理を実行する。
【００７１】
位置推定用ＥＣＵ４３は、ランドマークの移動を検知したか否かの判定を行う（Ｓ３１）。Ｓ３１でランドマークの移動を検知した場合には、評価値再設定処理を実行する（Ｓ３２）。ここでは、「ランドマークの高さによる評価」、「ランドマークの場所による評価」、「ランドマークの空間的な配置による評価」、「ランドマークの特性による評価」、「ランドマーク検出センサによる評価」、「ランドマークの移動実績による評価」に応じて、ランドマークの評価値Ｗ１〜Ｗ５，Ｗｆを再設定する。更に、「ランドマークの場所による評価」に基づいて評価値Ｗ２が変更になった場合には、同一領域内に存在するランドマークの評価値Ｗ２も変更する。
【００７２】
一方、Ｓ３１でランドマークの移動を検知したと判定されなかった場合、すなわち、新たなランドマークを検知した場合には、評価値設定処理を実行する（Ｓ３３）。ここでは、「ランドマークの属する種類による評価」に応じてランドマークの評価値を設定する。同一の種類のランドマークの評価値（過去のデータ）が保存されている場合には、過去の評価値を引き継ぐ。同一の種類のランドマークの評価値が保存されていない場合には、評価値Ｗ１〜Ｗ５を新たに設定する。
【００７３】
位置推定用ＥＣＵ４３は、評価値再設定処理、評価値設定処理を実行した後、ランドマークのデータ（地図データ）を更新する（Ｓ３４）。記憶部５は、更新されたランドマークのデータを地図データとして記憶する（Ｓ２５）。
【００７４】
この第２実施形態に係る自己位置推定装置４１によれば、第１実施形態の自己位置推定装置１と同様の効果を得ることができ、加えて、地図作成機能、地図更新機能を有しているため、実環境の変化に合わせて、ランドマークの評価値を設定することができる。従って、実環境の変化に対応したランドマークの評価値を用いて、移動体の自己位置を推定することができ、位置精度の向上を図られている。
【００７５】
以上、本発明をその実施形態に基づき具体的に説明したが、本発明は、上記実施形態に限定されるものではない。上記実施形態において、評価基準「ランドマークの高さによる評価」、「ランドマークの場所による評価」、「ランドマークの空間的な配置による評価」、「ランドマークの特性による評価」、「ランドマーク検出センサによる評価」に基づいて、ランドマークの評価値Ｗを設定しているが、その他の評価基準に基づいて、ランドマークの評価値（ランドマーク情報の信頼度）を設定してもよい。要は、信頼度の高いランドマーク情報の影響度を大きくして、移動体の自己位置を推定すればよい。
【００７６】
また、上記実施形態にあっては、複数の評価基準を用いてランドマークの評価値を設定しているが、一つの評価基準を用いて、ランドマークの評価値を設定してもよい。
【００７７】
また、上記第２実施形態にあっては、地図データ作成処理を位置推定用ＥＣＵによって実行しているが、地図データ作成処理を実行する地図データ作成用ＥＣＵと、位置推定用ＥＣＵとを別々に備える構成とし、地図データ作成用ＥＣＵ、位置推定用ＥＣＵ、地図データ記憶間でデータの送受信を行うようにしてもよい。なお、地図データ作成処理を移動体に搭載されたＥＣＵによって実行する必要はなく、移動体とは、別に設置されたＥＣＵによって地図作成、地図更新を実施してもよい。
【００７８】
また、上記実施形態にあっては、自己位置推定装置をロボット等の移動体に適用しているが、例えば、乗用車等のその他の移動体に適用してもよい。
【００７９】
また、上記実施形態にあっては、複数のランドマーク検出センサ２を備える自己位置推定装置としているが、ランドマーク検出センサを複数備える必要はなく、一つのランドマーク検出センサを備える自己位置推定装置でもよい。
【図面の簡単な説明】
【００８０】
【図１】本発明の第１実施形態に係る自己位置推定装置を示すシステム構成図である。
【図２】屋内環境を示す概略図であり、「ランドマークの高さによる評価」に基く評価値を設定するランドマークの一例を示すものである。
【図３】屋内環境を示す概略図であり、「ランドマークの場所による評価」に基く評価値を設定するランドマークの一例を示すものである。
【図４】屋内環境を示す概略図であり、「ランドマークの空間的な配置による評価」に基く評価値を設定するランドマークの一例を示すものである。
【図５】カメラによって撮像された画像の一例を示す概略図である。
【図６】記憶部に記憶された地図データの一例を示す概略図である。
【図７】位置推定用ＥＣＵで実行される位置推定処理の動作手順を示すフローチャートである。
【図８】本発明の第２実施形態に係る自己位置推定装置を示すシステム構成図である。
【図９】屋内環境を示す概略図であり、「ランドマークの移動実績による評価」に基く評価値を設定するランドマークの一例を示すものである。
【図１０】屋内環境を示す概略図であり、「ランドマークの場所による評価」に基く評価値を設定するランドマークの一例を示すものである。
【図１１】地図データの一例を示す表である。
【図１２】位置推定用ＥＣＵで実行される地図作成処理の動作手順を示すフローチャートである。
【図１３】位置推定用ＥＣＵで実行される地図更新処理の動作手順を示すフローチャートである。
【符号の説明】
【００８１】
１，４１…自己位置推定装置、２…ランドマーク検出センサ（検出手段）、３…位置推定用ＥＣＵ、５…記憶部（信頼度情報記憶手段）、７…地図照合部（照合手段）、８…位置推定部（自己位置推定手段）、４３…位置推定用ＥＣＵ、４７…評価値設定部（評価手段）。
【図１】
2008040677.tif 000005
【図２】
2008040677.tif 000006
【図３】
2008040677.tif 000007
【図４】
2008040677.tif 000008
【図５】
2008040677.tif 000009
【図６】
2008040677.tif 000010
【図７】
2008040677.tif 000011
【図８】
2008040677.tif 000012
【図９】
2008040677.tif 000013
【図１０】
2008040677.tif 000014
【図１１】
2008040677.tif 000015
【図１２】
2008040677.tif 000016
【図１３】
2008040677.tif 000017