JP2004294421A

JP2004294421A - 自己位置計測方法及び装置

Info

Publication number: JP2004294421A
Application number: JP2003144065A
Authority: JP
Inventors: Yoji Kuroda; 洋司黒田
Original assignee: Meiji University
Current assignee: Meiji University
Priority date: 2003-02-03
Filing date: 2003-05-21
Publication date: 2004-10-21
Anticipated expiration: 2023-05-21
Also published as: JP4046186B2

Abstract

【課題】安定して精度よく自己位置を計測する装置を提供すること。
【解決手段】移動前に、装置１自体に備えられた位置測定手段２，４５にて周囲の少なくとも一つの特徴点の相対位置を測定すると共に、装置１自体に備えられた姿勢検出手段３にて装置自身の姿勢情報を検出する移動前工程と、移動後に、位置測定手段２，４１にて特徴点の相対位置を測定すると共に、姿勢検出手段３にて装置自体の姿勢情報を検出する移動後工程と、装置自体に備えられた演算手段４５等を用いて、検出した姿勢情報から装置自身の移動前後における姿勢変化量である姿勢変化情報を算出すると共に、当該姿勢変化情報と特徴点の移動前後の各相対位置情報とに基づいて装置自体の移動前に対する移動後の自己位置を算出する演算工程とを備えた。
【選択図】図１

Description

【０００１】
【発明の属する技術分野】
本発明は、自己位置計測方法及び装置にかかり、特に、移動体や持ち運び可能な装置自体の移動前の位置に対する移動後の自己位置を計測する方法及び装置に関する。
【０００２】
【従来の技術】
従来より、装置自身の自己位置を測定する方法が研究され、それを実現する装置が開発されている。そして、移動体自体や人間等が持ち歩く物体に応用し、その移動後の位置を計測して、当該位置を繋ぎ合わせることにより、一定時間経過後の装置の移動経路や現在位置を計測することができる。
【０００３】
ここで、従来より用いられている自己位置測定方法として、画像を用いるものがある。そのような画像のみによって自己位置を測定するシステムは、環境上の少なくとも４つの地点の相対位置（あるいは相対位置の変化量）を常に正確にかつ安定に測定し続ける必要がある。しかし、４地点の位置すべてを常に安定に計測し続けることは、実際には非常に困難なことであり、一瞬でも３点以下になるとその時点で機能しなくなってしまう。仮に、４点の計測が出来たとしても、そのうち一点でも計測精度が低いと計測位置の精度が甚だしく劣化するという問題がある。
【０００４】
また、この手法では、４地点の座標から三角測量で自己位置を求めるので、必然的にその４地点は互いに離れているということとなる。従って、その４点は画面の周辺部付近に写っている場合には、カメラが移動すればその４点のうちいずれかはすぐに画面上から外れることとなる。これは４点を継続的に測定し続けることが構造的に困難であることを意味している。
【０００５】
この問題を克服するためには、複数のカメラを用いるか、あるいは、特殊な広角のレンズを用いる必要があった。但し、複数のカメラセットを用いればシステムが複雑になる上、カメラ間の精度を高く維持することが困難となる。また、広角のレンズを用いると精度が落ちる等の問題が生じる。また、別の問題として、計測画像中に移動物体がある場合は使用が困難であった。
【０００６】
そして、上記従来技術の問題点を解決すべく、特許文献１に示すような技術も開示されている。この特許文献１に示す技術は、走行中に周辺画像を撮影し、走行周辺環境の画像により自己位置を測定して目標に向かって移動するというものであり、自走車に搭載された自己位置測定装置である。この装置においては、実空間での位置が既知である複数の特徴物を画像上で検出し、それぞれの特徴物の実空間における位置と画像面における位置とに基づいて、自己位置を求めている。
【０００７】
【特許文献１】
特開平９−５３９３９号公報
【０００８】
【発明が解決しようとする課題】
しかしながら、上記特許文献１記載の従来技術にも以下のような不都合があった。すなわち、特許文献１のものは、特徴物の実空間での位置をあらかじめ装置に記憶しておかなければならず、特定の場所以外では、自己位置の計測ができないと問題があった。
【０００９】
また、上述したように、画像上において４点の特徴点を抽出する方法は自己位置計測の精度を維持することが困難であり、いずれにしても、自己位置を計測する従来技術には種々の問題が生じている。
【００１０】
【発明の目的】
本発明は、上記従来例の有する不都合を改善し、特に、あらゆる場所において、安定して精度よく自己位置を計測することができる方法及び装置を提供することをその目的とする。
【００１１】
【課題を解決するための手段】
そこで、本発明では、装置自体の移動前に対する移動後の自己位置を計測する方法であって、移動前に、装置自体に備えられた位置測定手段にて周囲の少なくとも一つの特徴点の相対位置を測定すると共に、装置自体に備えられた姿勢検出手段にて装置自身の姿勢情報を検出する移動前工程と、移動後に、位置測定手段にて特徴点の相対位置を測定すると共に、姿勢検出手段にて装置自体の姿勢情報を検出する移動後工程とを備えている。そして、移動後工程に続いて、装置自体に備えられた演算手段を用いて、検出した姿勢情報から装置自身の移動前後における姿勢変化量である姿勢変化情報を算出すると共に、当該姿勢変化情報と特徴点の移動前後の各相対位置情報とに基づいて装置自体の移動前に対する移動後の自己位置を算出する演算工程を備えた、という構成を採っている。
【００１２】
このような方法による構成を採ることにより、まず、位置測定手段にて移動前に少なくとも一つの特徴点の装置に対する相対位置を測定すると共に、姿勢検出手段にて装置自体の姿勢情報を検出する。そして、移動後にも、同一の特徴点の相対位置を測定して、装置自体の姿勢情報を検出する。その後、演算手段にて、移動前後の特徴点の各相対位置情報と自己の姿勢変化情報とを、あらかじめ定められた式に代入して演算することにより、装置自体の移動前の位置に対する移動後の自己位置を算出する。従って、少なくとも一つの特徴点の位置情報と、姿勢情報とを測定することで自己位置を測定することができるため、簡易かつ安定して位置を計測することができ、計測精度の向上をも図ることができる。
【００１３】
また、移動前工程は、位置測定手段にてあらかじめ定められた判断基準に基づいて周囲の任意の箇所を特徴点として選定する、という構成であると望ましい。これにより、あらゆる環境においても任意の特徴点を選定することで自己位置を計測することができ、適応性の向上を図ることができる。
【００１４】
また、移動前工程は、位置測定手段にてあらかじめ定められた判断基準に基づいて一つの特徴点を選定すると共に、移動後工程は、移動前工程にて選定した特徴点と同一の特徴点を抽出する、という構成であってもよい。これにより、上述したように一つの特徴点でも自己位置を計測することができるため、当該計測を実現しつつ、計測処理の情報量の減少を図り、演算手段の処理負担を抑制することができる。また、計測処理の迅速化を図ることができ、自己位置計測の実時間処理を実現する。
【００１５】
さらに、移動前工程は、あらかじめ定められた判断基準に基づいて複数の任意の特徴点を選定すると共に、それぞれの特徴点についての位置を測定し、移動後工程は、移動前工程にて選定した複数の特徴点と同一の特徴点をそれぞれ抽出して各特徴点についての位置を測定し、演算工程は、複数の特徴点毎に装置自体の移動前に対する移動後の自己位置を計測して、これら複数の計測値に基づいて最終的な自己位置を算出する、という構成とするとなお望ましい。
【００１６】
これにより、特徴点を複数選定し、各特徴点毎に自己位置を計測してそれら計測結果を総合して最終的な自己位置を計測することで、さらなる計測精度の向上を図ることができる。また、複数の特徴点を選定することにより、移動後に抽出できない特徴点が生じたとしても、他の特徴点から自己位置を計測することができるため、安定して計測を行うことができる。
【００１７】
また、上記複数の特徴点を用いて自己位置を測定する方法において、移動前工程及び移動後工程は、ステレオ視可能な複数のカメラによって構成された位置測定手段を用いて，各カメラによる複数の取得画像をマッチングすることにより各画像毎にそれぞれ対応する各特徴点を検出し，ステレオ視の原理により当該各特徴点の相対位置を測定すると共に、移動後工程は、各カメラにて取得した複数の画像をマッチングする際に算出する当該マッチングの一致度に関する値を，各特徴点毎に対応付けて位置測定手段あるいは演算手段に記憶し、演算工程は、複数の特徴点毎に計測した装置自体の移動前後における自己位置の各計測値に，各特徴点の一致度に関する値に応じた重みを付けて最終的な自己位置を算出する、という構成であると望ましい。このとき、演算工程は、マッチング時の一致度が高くなるにつれて各計測値に付ける重みが大きくなるよう設定する、こととするとなお望ましい。
【００１８】
このようにすることで、各特徴点毎に複数の得られた自己位置の測定値を総合して最終的な自己位置を算出する際に、精度よく検出できた特徴点から算出された装置の自己位置の計測値に大きな重みを付加し、当該計測値をより重視して最終的な自己位置を計測することとなる。従って、より信頼性の高い自己位置を算出することができ、計測制度の向上を図ることができる。
【００１９】
また、上記同様に複数の特徴点を用いて測定する場合に、移動前工程及び移動後工程は、画像取得手段にて構成された位置測定手段を用いて，取得画像の濃淡値に基づいて複数の特徴点を選定及び検出し，当該各特徴点の相対位置を測定すると共に、移動前工程は、各特徴点を選定する際に算出する当該各特徴点の濃淡値に関する情報である濃淡特徴量を，当該各特徴点に対応付けて位置測定手段あるいは演算手段に記憶し、演算工程は、複数の特徴点毎に計測した装置自体の移動前後における自己位置の各測定値に，各特徴点の濃淡特徴量に応じた重みを付けて最終的な自己位置を算出する、という構成であると望ましい。このとき、移動前工程は、濃淡特徴量を画像上の所定領域内における最大濃淡値と最小濃淡値との差で表すと共に、演算工程は、濃淡特徴量が大きくなるにつれて各計測値に付ける重みが大きくなるよう設定する、こととするとなお望ましい。
【００２０】
このようにすることで、濃淡差の大きい、より特徴のある特徴点から算出された装置の自己位置の計測値に大きな重みを付加し、当該計測値をより重視して最終的な自己位置を計測することとなる。従って、より信頼性の高い自己位置を算出することができ、計測制度の向上を図ることができる。
【００２１】
また、上記同様に複数の特徴点を用いて測定する場合に、移動後工程及び演算工程は、演算工程によって装置自体の移動前後における自己位置が算出された後に繰り返し作動すると共に，その際に，直前に作動した移動後工程にて抽出された複数の特徴点を移動前工程にて選定した複数の特徴点とみなして作動し、移動後工程は、各特徴点を抽出する際に，当該各特徴点を連続して検出した回数をカウントすると共に，当該回数の値を各特徴点毎に位置測定手段あるい演算手段に記憶し、演算工程は、自己位置の測定を複数回行った後に，複数の特徴点毎に計測した装置自体の移動前後における自己位置の各測定値に，各特徴点の検出回数に応じた重みを付けて最終的な自己位置を算出する、という構成であると望ましい。このとき、演算工程は、検出回数が多くなるにつれて各計測値に付ける重みが大きくなるよう設定する、こととするとより望ましい。
【００２２】
このようにすることで、継続して特徴点の追従を行い、装置の移動経路をも計測する構成において、連続して検出される信頼性の高い特徴点から算出された装置の自己位置の計測値に大きな重みを付加し、当該計測値をより重視して最終的な自己位置を計測することとなる。従って、より信頼性の高い自己位置を算出することができ、計測制度の向上を図ることができる。
【００２３】
さらに、上記同様に複数の特徴点を用いて測定し、また、継続して特徴点を追従して装置の移動経路をも計測する構成において、演算工程は、算出した装置自体の移動前後における最終的な自己位置に関する値を演算手段に記憶し、演算工程は、自己位置の測定を複数回行った後に、これまでの自己位置に関する値に基づいて装置自体の移動後の自己位置を予測して予測自己位置を算出する予測値算出工程と、複数の特徴点毎に計測した自己位置の各測定値と予測自己位置とを比較して，当該各特徴点毎にそのずれ量を算出する比較工程と、複数の特徴点毎に計測した自己位置の各測定値に，各特徴点毎に算出したずれ量に応じた重みを付けて最終的な自己位置を算出する最終自己位置算出工程と、を備えた構成であると望ましい。このとき、演算工程は、ずれ量が小さくなるにつれて各計測値に付ける重みが大きくなるよう設定する、こととすると望ましい。
【００２４】
このようにすることで、まず、過去の移動経路に基づいてあらかじめ定められた演算手法にて算出された予測値が算出される。そして、当該予測値と特徴点毎に算出された自己位置とが比較され、その差であるずれ量の少ない自己位置の計測値に大きな重みを付加し、当該計測値をより重視して最終的な自己位置を計測する。従って、ずれ量が大きく、次の自己位置として可能性の低い値の影響を抑制し、より信頼性の高い自己位置を算出することができ、計測制度の向上を図ることができる。
【００２５】
また、本発明では、装置自体の移動前に対する移動後の自己位置を計測する装置であって、装置周囲の少なくとも一つの特徴点の相対位置を測定する位置測定手段と、装置自身の姿勢情報を検出する姿勢検出手段と、これらの動作を制御して取得した情報を演算処理する演算手段とを備え、位置測定手段が、装置の移動前後におけるそれぞれの特徴点の相対位置を測定すると共に、姿勢検出手段が、装置自体の移動前後におけるそれぞれの姿勢情報を取得し、演算手段が、取得した姿勢情報の移動前後における変化量である姿勢変化情報を算出すると共に、当該姿勢変化情報と装置自体の移動前後における特徴点の各相対位置情報とに基づいて当該装置自体の移動前に対する移動後の自己位置を算出する機能を有する、という構成の自己位置計測装置をも提供している。
【００２６】
さらに、具体的には、装置自体の周囲画像を取得する画像取得手段と、装置自身の姿勢情報を検出する姿勢検出手段と、画像及び検出した姿勢情報を処理する演算手段とを備え、装置自体の移動前の対する移動後の自己位置を計測する装置であって、演算手段が、画像取得手段にて取得した画像を用いて周囲画像中の少なくとも一つの特徴点の相対位置を当該装置自身の移動前後においてそれぞれ測定する特徴点位置測定機能と、姿勢検出手段にて検出した当該装置自身の移動前後における姿勢情報からその変化量を算出する姿勢変化情報算出機能と、装置自体の移動前後の特徴点の各位置情報と姿勢情報変化量とを予め定められた式に代入することにより装置自体の移動量を算出する移動量算出機能と、を有し、これにより装置自体の移動前に対する移動後の自己位置を計測する、という自己位置計測装置をも提供している。
【００２７】
このような構成を採っても、上述同様の作用、効果を得ることができ、上記目的を達成することができる。
【００２８】
【発明の実施の形態】
〈第１の実施形態〉
以下、本発明の第１の実施形態について、図１乃至図６を参照して説明する。図１は、本実施形態における自己位置計測装置の動作の様子を示す概略図である。図２は、自己位置計測装置の構成を示すブロック図である。図３は、装置の自己位置を計測する手順を示すフローチャートである。図４は、装置にて画像処理をする時の動作を示すフローチャートである。図５乃至図６は、画像処理の様子を示す説明図である。
【００２９】
（全体構成）
本発明である自己位置計測装置は、本実施形態においては車両型の移動体１に備えられている。そして、移動体１自体の移動前の位置に対する移動後の自己位置を計測するものである。その構成の概略を、まず図１（ａ）を参照して説明する。
【００３０】
図１（ａ）に示す移動体１は、移動体１周囲の少なくとも一つの特徴点の当該移動体１に対する相対位置を測定する位置測定手段２と、移動体自身の姿勢情報を検出する姿勢検出手段（図示せず）と、これらの動作を制御して取得した情報を演算処理する演算手段（図示せず）とを備えている。これら各構成要素は、概ね以下の機能を有する。
【００３１】
位置測定手段２は、移動体１の移動前後において特徴点を抽出、追跡（トラッキング）して、当該特徴点の装置に対する相対位置を測定する。姿勢検出手段は、移動体自体の移動前後におけるそれぞれの姿勢情報を取得する。そして、演算手段は、取得した姿勢情報の移動前後における変化量である姿勢変化情報を算出すると共に、当該姿勢変化情報と移動体自体の移動前後における特徴点の各相対位置情報とに基づいて当該移動体自体の移動前に対する移動後の自己位置を算出する。
【００３２】
ここで、本実施形態では、具体的には図２に示すような構成を採っている。まず、位置測定手段２は、移動体１自体の周囲画像を取得する画像取得手段であるステレオカメラである。このステレオカメラ２は、図１（ａ）に示すように、移動体１のシャシ部１１から上方に延びる支柱１２の上端部に固定されて備えられている。そして、本実施形態では、ステレオカメラ２は２つのＣＣＤカメラにて構成されており、一方のカメラから取り込まれた画像をマスター画像、他方のカメラにて取り込まれた画像をスレーブ画像として取り扱う。なお、ステレオカメラは２つのカメラにて構成されることに限定されない。
【００３３】
また、移動体１自身の姿勢情報を検出する姿勢検出手段は、当該移動体１の姿勢角を検出する姿勢角センサ３である。この姿勢角センサ３は、図１（ａ）では図示しないが、移動体１のシャシ部１１に固定されて備えられている。さらに、ステレオカメラ２にて取り込んだ画像、及び、姿勢角センサ３にて検出した姿勢情報を処理する演算手段である演算処理部４１，４２，４３，４４，４５を、移動体１の内外に備えている。以下、これをさらに詳述する。
【００３４】
（移動体）
まず、移動体１の構成の概略を図１（ａ）に示すが、この移動体１は、シャシ部１１の左右にホイール１３，１４（２輪）を備えていて、これにより走行する車両である。例えば、月面などを探査する無人移動体である。但し、移動体１の構成はこれに限定されることなく、例えば、一般的な有人の自動車などの移動体であってもよい。
【００３５】
そして、この移動体１を走行させるべく、各ホイール１３ａ，１３ｂを駆動するＤＣモータ１３ａ，１４ａ、当該各モータの回転角度を検出するエンコーダ１３ｂ、１４ｂ、各エンコーダから検出値を読み取って上位コンピュータからの指令に基づいてＤＣモータ１３ａ，１４ａに回転制御指令を発するモータドライバとしてのＣＰＵ４３，４４が、シャシ部１１に備えられている。
【００３６】
（ステレオカメラ）
ステレオカメラ２は、上述したように移動体１のシャシ部１１の上部に備えられていて、移動体周囲のステレオ視差画像を取り込む。取り込んだ画像は、演算手段としての画像処理部４１などにて処理され、その一部はトランスミッター１５を介して遠隔地にあるメイン処理部１０のモニタ１０ｂに送信され、オペレータに表示される。また、画像処理部４１は、周囲画像からあらかじめ定められたアルゴリズムに基づくプログラムが作動することにより特徴点を抽出し、かかる特徴点の位置を表す三次元座標（ボディ座標系）を算出する機能を有する。すなわち、画像に関する演算手段としての画像処理部４１は、画像取得手段にて取得した画像を用いて周囲画像中の少なくとも一つの特徴点の相対位置を当該移動体１自身の移動前後においてそれぞれ測定する特徴点位置測定機能を備えている。その具体的な処理については動作説明時に詳述する。なお、ステレオカメラ２である画像取得手段と、取得した画像を処理する画像処理部４１等（メインコンピュータ４５を含む場合あり）とによる構成を、上記では位置測定手段と呼んでいる。
【００３７】
そして、画像から測定された特徴点の位置情報は、ハブ１６、無線ＬＡＮ１７，１０ａを介して遠隔地に位置するメイン処理部１０のメインコンピュータ４５に送信され、移動体１自身の自己位置を算出する際に用いられる。メイン処理部１０については後述する。
【００３８】
但し、上記特徴点の位置情報は、ハブ１６を介して移動体１に搭載された別のＣＰＵ４２に送信され、当該ＣＰＵ４２にて移動体１の自己位置が算出されてもよい。また、画像から特徴点を抽出する処理などの一連の処理が、遠隔地に位置するメイン処理部１０のメインコンピュータ４５にて実行されてもよい。すなわち、取得した画像情報自体を、ハブ１６から無線ＬＡＮ１７，１０ａを介してメインコンピュータ４５に送信してもよい。このようにしても、メインコンピュータ４５が遠隔地にあり、処理するコンピュータが異なるのみであって、本発明の構成としては同一である。
【００３９】
（姿勢角センサ）
姿勢角センサ３は、移動体１のシャシ部１１に固定して備えられており、本実施形態では、傾斜角センサと方位計を姿勢角センサ３として用いることで、シャシ部１１の向き、傾斜情報を取得する。そして、これらの情報はＣＰＵ４２が姿勢角センサ３の動作を制御することで当該姿勢角センサ３から取得し、その後は、ハブ１６、無線ＬＡＮ１７，１０ａを介してメインコンピュータ４５に送信され、当該コンピュータ４５にて自己位置の算出処理に用いられる。但し、傾斜角などの姿勢角に関する情報を検出したＣＰＵ４２が、後述するメインコンピュータ４５にて行われる処理すなわち前回検出した値と比較してその変化量を算出する処理を実行してもよい。その処理については後述する。
【００４０】
ちなみに、姿勢角センサ３として、上記傾斜角センサや方位計を用いることで、比較的安価に本発明にかかる装置を構成することができる。但し、姿勢角センサ３はこれに限定されるものではない。センサ３から検出した姿勢角情報からその変化量がわかればよく、必ずしも移動体１の絶対角を検出する必要がないため、ジャイロスコープなどを用いてもよい。具体的には、３軸のジャイロ値を積分してもよい。また、姿勢角が変化しないことが明確な場合、すなわち、水平面上を移動する場合には、方位角の１軸のみ計測すればよい。
【００４１】
ここで、上記ＣＰＵ４２は、メインコンピュータ４５からの指令に応じて、左右のホイール１３，１４の駆動状態を制御する他のＣＰＵ４３，４４に駆動指令を発する機能も有する。
【００４２】
（メイン処理部）
また、本実施形態では、無人の移動体１であるため、オペレータによる走行指令が入力され、当該走行指令に応じて移動体１１の走行状態を制御するメイン処理部１０が、移動体１と分離して設けられている。ここで、移動体１が自走式である場合には、自走するための制御プログラムを実行して、遠隔にて移動体１の走行状態を制御してもよい。但し、メイン処理部１０は移動体１自体に備えられていてもよい。
【００４３】
また、メイン処理部１０には、ＣＰＵやメモリなどを有するメインコンピュータ４５が搭載されており、無線ＬＡＮ１０ａといった通信手段を介して移動体１とデータ通信を行う。さらに、メイン処理部１０には、移動体１にて取り込まれ送信された画像を受信して表示するモニタ１０ｂが備えられている。これにより、オペレータは、移動体１の周囲の状況をモニタ１０ｂにて確認しながら走行指令を出すことができる。
【００４４】
ここで、メインコンピュータ４５は、主に、姿勢角センサ３にて検出し移動体１から送信された姿勢情報から、当該移動体１自身の移動前後における変化量である姿勢角変化情報を算出する姿勢変化情報算出機能と、移動体１自体の移動前後の特徴点の各位置情報と姿勢情報変化量とを予め定められた式に代入することにより当該移動体１自体の移動量を算出する移動量算出機能とを備えている。
【００４５】
具体的には、姿勢角変化情報とは、姿勢角センサ３にて検出したシャシ部１１の向き、傾斜角から、ロール角、ピッチ角、ヨー角の変化量を算出する。そして、移動量算出機能にて、当該ロール角等の情報と、移動前後における特徴点の各位置情報とを用いて、あらかじめメインコンピュータ４５に記憶されている算出式に代入することで、移動体１の移動前に対する移動後の三次元座標を得ることができる。
【００４６】
そして、算出した移動体１の移動量に関する情報を当該メインコンピュータ４５が記憶し、その後の移動量も蓄積することにより、移動体１の移動経路や現在位置を算出することができる。
【００４７】
ここで、メインコンピュータ４５は、上述した移動体１上の演算手段４１，４２，４３，４４が実行する処理を全て行ってもよい。いずれにしても、移動体１上の演算手段４１等で処理しようと、遠隔地にあるメインコンピュータ４５で処理しようとも、その構成に相違はない。
【００４８】
（動作）
次に、本発明の第１の実施形態における基本的な動作を、図３を参照して説明する。図３は、本実施形態における基本的な動作を示すフローチャートである。なお、以下に示す動作は、メインコンピュータ４５や移動体１内の各処理部（画像処理部４１やＣＰＵ４２，４３，４４など）に、あらかじめ各動作用プログラムが組み込まれることで種々の機能が構築されて実現される。また、以下の動作を実現するために必要なデータも、あらかじめメインコンピュータ４５などに記憶されている。
【００４９】
移動体１は、まず移動を開始する前、あるいは、移動中のあるタイミングにて、ステレオカメラ２にて周囲画像を取得する。そして、画像処理部４１にて取り込んだ画像を処理し、当該周囲画像中から少なくとも一つの特徴点（本実施形態では１つの特徴点）を抽出する。その後、移動体１から抽出した特徴点までの相対位置を測定し、三次元座標を算出する。また、これと共に、姿勢角センサ３にて移動体１自体の向き、傾斜角などの姿勢情報を検出する。そして、これらの情報は、移動体１からメイン処理部１０へと送信され、メインコンピュータ４５に記憶保持される（ステップＳ１、移動前工程）。
【００５０】
続いて、移動体１が移動し、あるいは、移動しなくても、あらかじめ定められたサンプリングタイムが経過すると（ステップＳ２にて肯定判断）、再度周囲画像を取り込むと共に、自己の姿勢情報を検出する。そして、当該画像においては、上記移動前工程にて抽出した特徴点と同一の特徴点を抽出し、その特徴点までの現在の相対位置を測定する。また、同時に、現在の自己の姿勢情報をも検出する。これらの情報（特徴点の相対位置情報、姿勢情報）は、メイン処理部１０に送信され、メインコンピュータ４５に記憶される（ステップＳ３、移動後工程）。
【００５１】
続いて、時間的に前後するそれぞれの時点における特徴点の相対位置情報と、移動体１自身の姿勢情報とがメインコンピュータ４５に送信されると、当該コンピュータ４５では、まず、姿勢情報から移動体自身の姿勢変化量である姿勢変化情報が算出される（ステップＳ４）。その後、この姿勢変化情報と、それぞれの時点における特徴点までの各相対位置情報とをあらかじめ定められた式（後述する数９式）に代入することで、移動体１自体の時間的に前後する先の時刻の位置に対する後の時刻の自己位置が算出される（ステップＳ５、演算工程）。
【００５２】
そして、上記処理を繰り返すことにより、それぞれの時点における自己位置を算出することができるため、移動体１の移動経路を算出することができる。具体的には、さらに次のサンプリングタイムが経過すると、その経過時における取得画像などが新たな後の時刻におけるものとなり、その際に上記において後の時刻のものが新たな先の時刻のものと更新されるため、かかる２点間における相対的な移動量を算出することができる。
【００５３】
ここで、上記ステップＳ５の演算工程にて用いる式について説明する。なお、ステレオカメラ２は他の実施形態において説明するように固定されていなくてもよいが、本実施形態においては、上述したように、移動体１にステレオカメラ２が固定されている場合について説明する。そして、移動前（先）の時刻をｔ、移動後（後）の時刻をｔ＋１として、移動前に図１（ａ）に位置する移動体１が、図１（ｂ）に位置するよう移動した場合について説明する。
【００５４】
図１において、
Ｘ_Ｂ０：時刻ｔのときのボディ座標系
Ｘ_Ｃ０：時刻ｔのときのカメラ座標系
Ｘ_Ｂ１：時刻ｔ＋１のときのボディ座標系
Ｘ_Ｃ１：時刻ｔ＋１のときのカメラ座標系
^Ｂ０Ｘ_Ｐ：時刻ｔのときの点Ｐのボディ座標
^Ｃ０Ｘ_Ｐ：時刻ｔのときの点Ｐのカメラ座標
^Ｂ１Ｘ_Ｐ：時刻ｔ＋１のときの点Ｐのボディ座標
^Ｃ１Ｘ_Ｐ：時刻ｔ＋１のときの点Ｐのカメラ座標
φ：移動体１の移動前後における姿勢角（ロール角）の変化量
θ：移動体１の移動前後における姿勢角（ピッチ角）の変化量
ψ：移動体１の移動前後における姿勢角（ヨー角）の変化量
である。
【００５５】
今、Ｘ_Ｃ０からＸ_Ｂ０への同時変換行列を^Ｂ０ _Ｃ０Ｓとすると、移動体１に対してカメラが固定であれば、移動の前後で^Ｂ０ _Ｃ０Ｓは不変（つまり^Ｂ１ _Ｃ１Ｓ＝^Ｂ０ _Ｃ０Ｓ）であるから、以下の式のようになる。
【００５６】
【数１】

【００５７】
【数２】

【００５８】
ちなみに、上記同時変換行列^Ｂ０ _Ｃ０Ｓは、ステレオカメラ２が固定されていることにより、あらかじめ定められた行列である。
【００５９】
また、Ｘ_Ｂ０とＸ_Ｂ１の間の同時変換行列を^Ｂ０ _Ｂ１Ｓ、並進変換行列を^Ｂ０ _Ｂ１Ｔ、回転変換行列を^Ｂ０ _Ｂ１Ｒとし、Ｘ_Ｂ０とＸ_Ｂ１の間の並進移動量をｔ_ｘ，ｔ_ｙ，ｔ_ｚとすれば、
【００６０】
【数３】

【００６１】
【数４】

【００６２】
【数５】

【００６３】
【数６】

となる。ここで、
【００６４】
【数７】

とすれば、
【００６５】
【数８】

となるので、
【００６６】
【数９】

と表せる。
【００６７】
従って、あらかじめ定められた^Ｂ０ _Ｃ０Ｓを含む数９式に、測定した点Ｐの座標である^Ｃ０Ｘ_Ｐ及び^Ｃ１Ｘ_Ｐ，さらに、姿勢角の変化量にて表される^Ｂ０ _Ｂ１Ｒを代入することで、移動体１の時刻ｔに対する時刻ｔ＋１のときの移動量（ｔ_ｘ，ｔ_ｙ，ｔ_ｚ）を求めることができる。
【００６８】
続いて、図３のステップＳ１及びステップＳ３に示す特徴点の位置測定の手法について、図４乃至図６を参照して詳述する。図４は、特徴点の位置測定の動作を示すフローチャートである。図５乃至図６は、ステレオカメラ２にて取得した画像の処理の様子を示す図である。
【００６９】
まず、特徴点の位置測定の概略を説明する。基本的には、特徴点を選定し、当該特徴点の位置を測定する（ステップＳ１３，１４，１５）、という手順となるが、移動後における特徴点の位置測定では移動前に選定した特徴点を追跡（トラッキング）してその位置を測定する（ステップＳ１６，１４，１５）、という手順となる。はじめに、移動体１が移動前（先）の位置にいる場合の特徴点の抽出、及び、その位置測定の動作について説明する。
【００７０】
まず、移動体１は、メインコンピュータ４５、あるいは、画像処理部４１の指示により、ステレオカメラ２にて周囲の画像を取得する（ステップＳ１１、図５（ａ））。このとき、一方のカメラから取得したマスター画像と、他方のカメラから取得したスレーブ画像とを、図５（ａ）に示すように、一つのフレームメモリ上の奇数フィールド、偶数フィールドにそれぞれ格納する。なお、これにより、ＮＴＳＣ用の画像処理装置において、構成のコンパクト化を図ることができる。
【００７１】
ここで、本実施形態では、ステレオ視した画像から圧縮によりマスター画像を消去してスレーブ画像を取得し（図５（ｂ））、このスレーブ画像を画像処理部４１に一時的に記憶する。これと共に、画像の偶数ライン、奇数ラインを調整し（図５（ｃ））、圧縮によりスレーブ画像を消去し、マスター画像を取得する（図５（ｄ））。そして、このマスター画像を、スレーブ画像同様に画像処理部４１に一時的に記憶すると共に、当該マスター画像のコピーを（図５（ｅ））、移動前（先の時刻）におけるマスター画像として、画像処理部４１あるいはメインコンピュータ４５に別途記憶しておく。これは、移動後にトラッキングするはずの特徴点を見失った時などに用いる画像である。
【００７２】
ここで、取得したスレーブ画像（図５（ｂ））とマスター画像（図５（ｄ））とを、メイン処理部１０に送信してモニタ１０ｂに表示する。これにより、遠隔地にいるオペレータは、移動体１から見た周辺状況を視認することができる。
【００７３】
ちなみに、画像処理部４１に記憶されたマスター画像などの画像は、メインコンピュータ４５に記憶されてもよい。これは、特徴点の抽出処理を画像処理部４１で行うか、あるいは、メインコンピュータ４５で行うかの違いによるが、いずれの演算手段で行っても構成に相違はない。従って、この時点では、マスター画像（ｄ）、スレーブ画像（ｂ）、マスター画像のコピー（ｅ）、輪郭抽出画像（ｇ）が画像処理部４１あるいはメインコンピュータ４５に記憶されていることとなる。
【００７４】
続いて、今は移動前における特徴点の抽出処理であり、いまだ特徴点（トラッキングポイント）を選定していない状況にあるので、新たな特徴点の選定である抽出処理を行う（特徴点の更新要求あり（ステップＳ１２で肯定判断））。このため、本実施形態では、一時的に保持しているマスター画像（図５（ｄ））を二値化処理し（図５（ｆ））、輪郭抽出処理を行う（図５（ｇ））。この画像に基づいて、あらかじめ定められた判断基準に適合する箇所を特徴点として選定すべく、図６に示す処理を行う。但し、特徴点を選定する手順は、以下に示すものに限定されない。
【００７５】
まず、図６（ａ）に示すように、輪郭抽出画像を９つの領域に分割する。なお、分割領域に画面の淵を含ませないようにしたのは、移動後に特徴点が当該画像から外れてしまうのを防ぐためである。そして、ある領域において矩形テンプレートウインドウＷ１（例えば、２０×２０ピクセル）を設定し、当該テンプレートウインドウを、符号Ｗ２，Ｗ３に示すように左上から右下まで数ピクセルずつ移動させて、当該テンプレート内の濃淡特徴量を抽出する（図６（ｂ））。このとき、対象となる画像は２値の輪郭画像であるから、テンプレート内に白い輪郭線がある場合のみ特徴量を検出し、そのときのテンプレート左上の点の座標を保存しておく（例えば、テンプレートウインドウＷ２についての点Ｃ）。なお、処理対象となる領域は、９つのうち少なくとも一つでよく、図６（ｂ）に示すように左上の領域のみを処理するようあらかじめ定めておいてもよい。但し、処理領域については、他の実施形態において説明するように、移動体１の動作状況に応じた箇所の複数の領域を処理対象としてもよい。
【００７６】
このようにして、特徴量の検出できた全ての点の座標を保存したら、二値画像ではないマスター画像において保存した各座標におけるテンプレート内の濃淡特徴量を抽出する（図６（ｃ），（ｄ））。本実施形態では、濃淡特徴量を、テンプレートＷ２内で最大濃度の画素と最小濃度の画素との濃淡差、とする。そして、特徴量（濃淡差）が最大となるテンプレートを特徴点とし、その点の座標を保存しておく（特徴点の検出、ステップＳ１３）。但し、この特徴点を算出する手法は一例であって、この手法に限定されるものではない。
【００７７】
続いて、マスター画像において抽出した特徴点に対応する点（マッチングポイント）を、スレーブ画像上で検索する（ステップＳ１４）。このため、まず、マスター濃淡画像（図５（ｄ））において特徴点（特徴点に対応するテンプレートウインドウＷ２）の濃淡特徴量を抽出し、スレーブ濃淡画像（図５（ｂ））上で正規化相関をとる。これによりスレーブ画像上において特徴点に対応する点が見つかったらその点の座標を保存しておく。なお、正規化相関によるマッチングについては、従来より用いられている周知技術であるので、その詳細な説明は省略する。
【００７８】
その後、マスター画像上で検出した特徴点（トラッキングポイント）と、スレーブ画像上で検出した特徴点に対応する点（マッチングポイント）との座標から、画像処理部４１あるいはメインコンピュータ４５などの演算装置にて、ステレオ視の原理より特徴点の三次元座標が計測される（ステップＳ１５）。計測された三次元座標は、移動前すなわち図１（ａ）における時刻ｔのときの点Ｐのカメラ座標^Ｃ０Ｘ_Ｐとして、メインコンピュータ４５に記憶される。ここで、ステレオ視による三次元座標の計測については、周知技術であるのでその説明は省略する。
【００７９】
ちなみに、移動前における特徴点の抽出処理でなくとも、前回の画像において抽出した特徴点のトラッキングに失敗したり、何らかのエラーが出たために特徴点の更新要求がある場合には、別途記憶しておいた先の位置におけるマスター画像（図５（ｅ））を用いて、特徴点の抽出処理が行われる（ステップＳ１２で肯定判断、ステップＳ１３）。
【００８０】
そして、移動体１が移動した後、すなわち、前回の画像取得から所定時間が経過して時刻がｔ＋１となった時（図１（ｂ））には、再度ステレオカメラ２にて画像を取得し（ステップＳ１１）、特徴点の追跡を行う。かかる場合には、特徴点の更新要求がないため（ステップＳ１２にて否定判断）、新たに取得した画像中から先の画像にて抽出した特徴点と同一の特徴点のマッチングによる検出を行う（ステップＳ１６，Ｓ１４）。具体的には、先のマスター濃淡画像中において抽出した特徴点の濃淡特徴量を抽出し、現在のマスター濃淡画像上で正規化相関をとる。そして、対応する点が検出できたらその点の座標を保存しておく（ステップＳ１６）。また、当該現在のマスター画像中の特徴点に対応する点を、上述したようにスレーブ画像上で正規化相関をとり、対応する点が検出できたらその点の座標を保存しておく（ステップＳ１４）。
【００８１】
このようにして、移動後（時刻ｔ＝ｔ＋１）におけるマスター、スレーブの各画像上の特徴点の座標から、移動体１から見た点Ｐのカメラ座標^Ｃ１Ｘ_Ｐを、ステレオ視の原理より計測する（ステップＳ１５）。
【００８２】
ちなみに、上述したように、画像取得時（ステップＳ１１）には、移動前（時刻ｔ＝ｔ）、移動後（ｔ＝ｔ＋１）のいずれのときにおいても、移動体１の姿勢角の検出が行われる。かかる情報は、移動体自体のＣＰＵに保持されていてもよく、メインコンピュータ４５に記憶されてもよい。そして、移動後の姿勢角情報が検出された後には、移動前と比較してその変化量（φ，ψ，θ）が算出される。これら変化量の各値は、上述した数５式に示すように、回転変換行列を^Ｂ０ _Ｂ１Ｒとして表される。
【００８３】
このようにして測定された^Ｃ０Ｘ_Ｐ，^Ｃ１Ｘ_Ｐ，^Ｂ０ _Ｂ１Ｒから、あらかじめ定められている^Ｂ０ _Ｃ０Ｓを含む数９式を用いて移動体１の並進移動量がメインコンピュータ４５などの演算手段にて算出される。そして、引き続き、移動後（ｔ＝ｔ＋１）の位置を新たな移動前の位置とし、その後の移動後の位置を新たな移動後（ｔ＝ｔ＋２）とすることで、この間の移動体１の移動量を測定することができる。これらを繋ぎ合わせることで、移動体１の移動経路及び現在位置を計測することができる。
【００８４】
従って、移動体１の姿勢角と、当該移動体１に搭載されたカメラ２にて、少なくとも一つの特徴点を追跡することで自己位置を計測することができるため、安定して自己位置の計測を行うことができると共に、その計測精度の向上を図ることができる。
【００８５】
〈第２の実施形態〉
次に、本発明の第２の実施形態について、図７を参照して説明する。図７は、本発明の第２の実施形態における動作の様子を示す概略図である。特に、図７（ａ）は、時刻ｔの時の様子を示す図であり、図７（ｂ）は、時刻ｔ＋１の時の様子を示す図である。
【００８６】
なお、本実施形態における以下の動作も、上述した第１の実施形態と同様に、メインコンピュータ４５や移動体１内の画像処理部４１やＣＰＵ４２，４３，４４内に、あらかじめ各動作用プログラムが組み込まれることで実現される。
【００８７】
本実施形態においては、上述した第１の実施形態における移動体１すなわち自己位置計測装置とほぼ同一の構成を有しており、また、ほぼ同一の方法にて自己位置を計測する。但し、本実施形態では、ステレオカメラ２０が移動体１に固定されていない点で、上記第１の実施形態とは異なる。
【００８８】
図７（ｂ）に示すように、ステレオカメラ２０を移動体１に固定せず、例えば、図２に示すメイン処理部１０からオペレータが遠隔操作することにより可動可能である。すると、当該ステレオカメラ２０のパン・チルト角や、高さなどを変えることにより、より有益な視覚情報（画像）を得ることができる場合がある（符号２０’は、ステレオカメラが可動した時の様子を示す）。そして、このような場合にも、以下の算出式を用いることで、移動体１の並進移動量を求めることができる。ここで、ステレオカメラの固定位置に対する可動量、すなわち、図７（ｂ）の符号２０に示す位置から符号２０’の位置に可動したときの三次元の並進移動量（ａ，ｂ，ｃ）及び回転移動量（ロール角α、ピッチ角β、ヨー角γ）は、ポテンショメータやロータリーエンコーダなどのセンサを当該カメラに備えることで、検出することができる。
【００８９】
図７において、
Ｘ_Ｃ１：カメラが固定である場合の時刻ｔ＋１におけるカメラ座標系
Ｘ_Ｃ１’：カメラが固定でない場合の時刻ｔ＋１におけるカメラ座標系
^Ｃ１Ｘ_Ｐ：カメラが固定である場合の時刻ｔ＋１時の点Ｐのカメラ座標
^Ｃ１’Ｘ_Ｐ：カメラが固定でない場合の時刻ｔ＋１時の点Ｐのカメラ座標
である。そして、移動の前後でのステレオカメラの移動体ボディ座標に対するカメラ座標の並進移動量がａ，ｂ，ｃであり、回転移動量がα，β，γであるとすると、Ｘ_Ｃ１’からＸ_Ｃ１への同時変換行列^Ｃ１ _Ｃ１’Ｓは、
【００９０】
【数１０】

【００９１】
【数１１】

【００９２】
【数１２】

【００９３】
上記のようになるので、オペレータの操作によるステレオカメラの移動体ボディ座標に対するカメラ座標の並進移動量ａ，ｂ，ｃ及び回転移動量α，β，γの値を用いることで、その同時変換行列^Ｃ１ _Ｃ１’Ｓを算出することができる。従って、移動体１の並進移動量は、ステレオカメラ２０が固定のときと同様に、
【００９４】
【数１３】

【００９５】
となる。従って、まず、カメラが可動したときの並進移動量（ａ，ｂ，ｃ）、回転移動量（α，β，γ）を用いて^Ｃ１ _Ｃ１’Ｓを算出し、^Ｂ０ _Ｃ０Ｓと共に数１３式に代入しておき、当該式をあらかじめ定められた式として用いる。そして、この数１３式に第１の実施形態と同様に、移動前後における移動体１から特徴点である点Ｐまでの位置座標と、移動体の姿勢角変化量を代入することで、当該移動体１の自己位置を計測することができる。
【００９６】
〈第３の実施形態〉
以下、本発明の第３の実施形態について、図８を参照して説明する。本実施形態においては、上述した他の実施形態とほぼ同一の構成を有しているが、自己位置を計測する方法が、以下の点で異なる。なお、以下の動作は、画像処理部４１やメインコンピュータ４５などの演算手段に以下の方法を実現すべく演算を行うプログラムが組み込まれることで実現される。
【００９７】
まず、移動体１は、移動前工程にて、あらかじめ定められた判断基準に基づいて複数の特徴点を選定すると共に、それぞれの特徴点の相対位置を測定する。例えば、上述したように、取得画像を９つの領域に分割して、その中から４つの領域を選択し、それぞれの領域から特徴点を抽出する。４つの領域の選択は、移動体１の動作モードに応じて行うが、その一例を図８（ａ）〜（ｄ）に示す。これらの図において、それぞれ斜線領域が特徴点を抽出する領域であるが、図８（ａ）は、移動体が前進する場合、図８（ｂ）は後退する場合、図８（ｃ），（ｄ）は、それぞれ移動体が右回転、左回転する場合である。
【００９８】
そして、上述したように、マスター画像において特徴点の抽出を行ったら、スレーブ画像において特徴点のマッチングを行い、当該特徴点の座標を計測する。そして、これは４点についてそれぞれ行う。その後、移動体の移動後には、上記抽出した４つの特徴点をトラッキングし、それぞれの座標を計測する。このときもその計測手法は、上述した手法と同様である。
【００９９】
ここで、例えば、移動体１が前進する場合には、図８（ａ）の斜線４つの領域を特徴点の抽出領域として選択したが、移動体の移動方向が矢印Ｙ１方向であると特徴点の移動方向が矢印Ｙ２となるため、特徴点が画像内に収まり、トラッキングを継続することができる。そして、それぞれの場合も同様に、移動体が移動しても特徴点が領域内に収まるような処理領域となっている。
【０１００】
そして、それぞれの特徴点毎に、上述した手法と同様の手法にて、移動体１の移動前に対する移動後の自己位置を算出する。このとき、複数の特徴点にうち、トラッキングができない点も生じてくる。かかる場合には、より長い期間トラッキングを継続した点を信頼し、移動量を算出する。ちなみに、トラッキングにて特徴点を見失った場合には、新たな特徴点の更新、抽出要求が出され、抽出された新たな特徴点について位置計測が行われる。
【０１０１】
このようにすることにより、複数の特徴点をトラッキングすることで計測値の高精度化を図ることができると共に、取得画像に対する処理対象領域を必要の範囲内にすることができ、装置の処理負担を抑制することができる。また、移動体の動作モードに応じて処理領域を設定することで、特徴点のトラッキングを長時間継続することができ、移動体の位置計測精度のさらなる向上を図ることができる。
【０１０２】
ここで、上述した特徴点の抽出は、４点に限定されるものではない。上述した第１の実施形態のように１点でもよく、他の数の複数点でもよい。また、４点のうち信頼できる特徴点における計測データを採用することに限定されず、複数の特徴点についてその平均値を採ってもよく、また、最小自乗法などを用いて計測値を算出してもよい。
【０１０３】
〈第４の実施形態〉
次に、本発明の第４の実施形態について説明する。本実施形態では、複数の特徴点を抽出して移動体自体の自己位置を算出するものであるため、上述した第３の実施形態とほぼ同様の構成であるが、さらに、以下の構成を採っている。なお、以下の説明においては、図１乃至図８を必要に応じて用いることとする。
【０１０４】
また、以下に示す動作は、上述した他の実施形態と同様に、メインコンピュータ４５や移動体１内のＣＰＵなどに、あらかじめ各動作用プログラムが組み込まれることで実現できる。すなわち、メインコンピュータ４５や移動体１内の画像処理部、ＣＰＵなどに、以下の動作を実現する機能が構築されており、当該メインコンピュータ４５等に位置測定に関する各データが送信され、以下の処理が行われる。
【０１０５】
本実施形態では、まず、上述した他の実施形態と同様に、位置測定手段２はステレオ視可能な複数のカメラによるステレオカメラにて構成されており、図３のステップＳ１及びステップＳ３に示す上記移動前工程及び移動後工程では、ステレオカメラを用いて，各カメラによる複数の取得画像をマッチングすることにより各画像毎にそれぞれ対応する各特徴点を検出し，ステレオ視の原理により当該各特徴点の相対位置を測定する。その具体的な相対位置の測定手法は、上述したので省略する。
【０１０６】
そして、図３のステップＳ３に示す移動後工程では、各カメラにて取得した画像をマッチングする際に、当該マッチングによる一致度に関する値を算出して各特徴点毎に対応付けて演算手段に記憶する。具体的には、移動体１が所定のサンプリングタイム経過後（移動後）に、画像を取得して特徴点の追跡（トラッキング）を行うが、まず、一方のカメラにて取得したマスター画像から、上述した濃淡差を濃淡特徴量として移動前工程において選定した特徴点と同一の特徴点を検出する（図４のステップＳ１６）。その後、当該マスター画像上の特徴点に対応する点を、スレーブ画像で正規化相関をとり、マッチング処理を行うことにより検出する（図４のステップＳ１４）。このとき、正規化相関値は、マッチングした画像の一致度を表しており、０〜１の間の値をとるよう設定されている。例えば、完全に画像パターンが一致する場合に１となるようにする。本実施形態では、かかる値を「重み」として用い、複数の特徴点から求められた各自己位置である移動量のうち、どの移動量を重視して採用するか、ということの割合を示す値となる。
【０１０７】
従って、演算工程では、複数の特徴点毎に計測した自己位置の各計測値に，各特徴点の一致度に関する値に応じた重みを付けて最終的な自己位置を算出する。そして、このとき、特徴点を検出する際のマスター画像とスレーブ画像とのマッチングによる一致度が高くなるにつれて各計測値に付ける重みが大きくなるよう設定される。
【０１０８】
具体的には、ｎ個の特徴点がトラッキングできている場合に、上述したように、数９式あるいは数１３式を用いて各特徴点の相対位置から算出した移動体１の移動量ｘ_１〜ｎ（ここでは、ｘ方向の移動量のみを考える）に、上記相関値ｃ_１〜ｎを重みとして付加し、これらの加重平均ｍ_ｗを数１４式に示すように算出する。
【０１０９】
【数１４】

【０１１０】
そして、各特徴点毎に、各移動量（ｘ，ｙ，ｚ）それぞれの加重平均ｍ_ｗを求め、かかる値を最終的な移動量の計測値とする。
【０１１１】
これにより、ステレオカメラによるマッチング時の一致度が高い特徴点、すなわち、相関値が大きい特徴点から算出された移動量の割合が大きく反映されて、移動体１の最終的な移動量が算出されることとなる。そして、相関値が大きい特徴点は、静止物体上の特徴点である可能性が高いため、他の移動物体をトラッキングして移動量が算出されることが抑制されるため、より精度よく、移動体の現在位置を計測することができる。
【０１１２】
〈第５の実施形態〉
次に、本発明の第５の実施形態について説明する。本実施形態では、上記第４の実施形態と同様に、複数の特徴点を抽出して各特徴点毎に装置自体の移動量を算出し、特徴点毎の移動量に重みを付けて最終的な自己位置を算出する、という構成である。但し、本実施形態では、上記数１４式に示す加重平均を求める際の重みの設定方法が異なり、以下の構成を採っている。なお、以下の説明においては、図１乃至図８を必要に応じて用いることとする。
【０１１３】
また、以下に示す動作は、上述した他の実施形態と同様に、メインコンピュータ４５や移動体１内の画像処理部、ＣＰＵなどに、あらかじめ各動作用プログラムが組み込まれることで実現できる。すなわち、メインコンピュータ４５や移動体１内のＣＰＵなどに、以下の動作を実現する機能が構築されている。
【０１１４】
本実施形態においては、図３のステップＳ１及びステップＳ３に示す上記移動前工程及び移動後工程では、画像取得手段であるステレオカメラにて構成された位置測定手段２を用いて，取得画像の濃淡値に基づいて複数の特徴点を選定及び検出し，当該各特徴点の相対位置を測定する。
【０１１５】
このとき、移動前工程は、各特徴点を選定する際に，当該各特徴点の濃淡値に関する情報、具体的には、マスター画像上における所定領域の最大濃淡値と最小濃淡値との差を濃淡特徴量として、最も高い領域上の点を特徴点として選定する（図４のステップＳ１４）が、本実施形態では、特に、選定した複数の特徴点毎に対応付けて、算出した濃淡特徴量をメインコンピュータ４５や移動体１内のＣＰＵ、あるいは、移動体１内に設けられた所定の記憶部などに記憶する。
【０１１６】
また、演算工程では、複数の特徴点毎に計測した自己位置の各測定値に，各特徴点の濃淡特徴量に応じた重みを付けて最終的な自己位置を算出する。このとき、重みは０〜１の値であるが、濃淡特徴量が大きければ大きいほど、１に近づく値として設定される。具体的には、数１５式に示す式にて算出する。なお、ｗが重みとなり、数１４式におけるｃと同様に働く。すなわち、ｗは、それぞれ特徴点の数ｎだけ算出され、ｗ_１〜ｎまで算出される。また、ｇは濃淡差であり、これも各特徴点毎に設定される。
【０１１７】
【数１５】

【０１１８】
なお、重みを上式のように算出するのは、画像の濃度は通常８ビットデータとして扱われ、濃淡差は０〜２５５の値をとる。従って、これを０〜１の値に変換するためである。
【０１１９】
このようにして求めた重みｗ（ｗ_１〜ｎ）は、特徴点毎に求められ、上記数１４式と同様に加重平均が算出される。これにより、画像の濃度が急激に変化する部分（エッジ）を特徴点として選定していることとなり、かかる特徴点の信頼度は高いと考えられる。また、このような特徴点は静止物体上の特徴点であるとも考えられる。従って、濃淡特徴量が大きいものの移動量の割合を大きく反映して最終的な移動量を算出することで、他の移動物体の影響を抑制して、より精度よく移動体１の現在位置を計測することができる。
【０１２０】
〈第６の実施形態〉
次に、本発明の第６の実施形態について説明する。本実施形態では、上記第４乃至５の実施形態と同様に、複数の特徴点を抽出して各特徴点毎に装置自体の移動量を算出し、特徴点毎の移動量に重みを付けて最終的な自己位置を算出する、という構成である。但し、本実施形態では、数１４式において移動体１の最終的な移動量である加重平均を求める際の重みの設定手法が異なり、以下の構成を採っている。なお、以下の説明においては、図１乃至図８を必要に応じて用いることとする。
【０１２１】
また、以下に示す動作は、上述した他の実施形態と同様に、メインコンピュータ４５や移動体１内のＣＰＵなどに、あらかじめ各動作用プログラムが組み込まれることで実現できる。すなわち、メインコンピュータ４５や移動体１内のＣＰＵなどに、以下の動作を実現する機能が構築されている。
【０１２２】
本実施形態における図３のステップＳ１及びステップＳ３に示す移動後工程及び演算工程は、演算工程によって自己位置の算出された後に繰り返し作動すると共に，その際に，直前に作動した移動後工程にて抽出された複数の特徴点を移動前工程にて選定した複数の特徴点として作動する。すなわち、第１の実施形態においても説明したように、移動体１が移動中に、連続して特徴点の追従を行い、その移動量を積算していくことで経路を算出する構成となっている。
【０１２３】
そして、特に、移動後工程では、各特徴点を抽出する際に，当該各特徴点が連続して検出された回数の値を、特徴点に対応させてメインコンピュータ４５や移動体１内の画像処理部など、あるいは、所定の記憶部に記憶する。具体的には、すべての特徴点毎に連続してトラッキングした回数を積算し、特徴点毎に記憶する。このとき、例えば、特徴点の識別番号及び当該特徴点の濃淡特徴量と共に記憶することで、他の特徴点とのトラッキング回数を区別して記憶することができる。そして、新たに選定された特徴点は、そのトラッキング回数が初期化されて、１回からカウントされ、トラックキング回数が増える毎に記憶されている値が更新される。また、これらトラッキング回数のうち、最も大きい値は、最高回数はｌ_ｍａｘとして別途メインコンピュータ４５や画像処理部、さらには、移動体１内の別の記憶部などに記憶される。この最高回数の値は、いずれかの特徴点のトラッキング回数が更新される毎に当該各特徴点のトラッキング回数と比較され、かかる値が最高回数の値よりも大きい場合には更新される。
【０１２４】
このようにして計測されたトラッキング回数は、最終的な移動量を算出する際に利用される。すなわち、演算工程では、自己位置の測定を複数回行った後に，複数の特徴点毎に計測した自己位置の各測定値に，各特徴点の検出回数値に応じた重みを付けて最終的な自己位置を算出する。このとき、重みｗは、トラッキング回数をｌ、その最大値をｌ_ｍａｘとすると、数１６式にように表され、すなわち、検出回数値が多くなるにつれて各計測値に付ける重みが大きくなるよう設定される。これにより、重みは０〜１の間に設定され、最大回数をカウントしている特徴点については、ｌ_ｍａｘ＝ｌとなり、その重みが１となる。なお、ｗは、数１４式におけるｃと同様に働く。すなわち、ｗは、それぞれ特徴点の数ｎだけ算出され、ｗ_１〜ｎまで算出される。また、ｌの値も各特徴点毎に設定される。
【０１２５】
【数１６】

【０１２６】
その後は、上述した第５の実施形態と同様に、求めた重みｗを用いて上記数１４式のように加重平均が算出される。これにより、視界から外れていないにも関わらず、少ない回数しか連続してトラッキングできないような不安定な特徴点から求められた移動量の影響を抑制することができ、一方で、連続して多くトラッキングできた特徴点からの移動量の割合が大きく反映されるため、信頼性の高く精度のよい移動体の現在位置を計測することができる。なお、上述したような不安定な特徴点は、他の移動物体上の特徴点であるとも考えられるため、そのような移動物体の影響を抑制することができる。
【０１２７】
〈第７の実施形態〉
次に、本発明の第７の実施形態について説明する。本実施形態では、上記第４乃至６の実施形態と同様に、複数の特徴点を抽出して各特徴点毎に装置自体の移動量を算出し、特徴点毎の移動量に重みを付けて最終的な自己位置を算出する、という構成である。但し、本実施形態では、数１４式において移動体１の最終的な移動量である加重平均を求める際の重みの設定手法が異なり、以下の構成を採っている。なお、以下の説明においては、図１乃至図８を必要に応じて用いることとする。
【０１２８】
また、以下に示す動作は、上述した他の実施形態と同様に、メインコンピュータ４５や移動体１内の画像処理部、ＣＰＵなどに、あらかじめ各動作用プログラムが組み込まれることで実現できる。すなわち、メインコンピュータ４５や移動体１内の画像処理部やＣＰＵなどに、以下の動作を実現する機能が構築されている。
【０１２９】
まず、本実施形態においては、上記第６の実施形態と同様に、図３のステップＳ１及びステップＳ３に示す移動後工程及び演算工程は、演算工程によって自己位置の算出された後に繰り返し作動すると共に，その際に，直前に作動した移動後工程にて抽出された複数の特徴点を移動前工程にて選定した複数の特徴点として作動する。すなわち、第１の実施形態においても説明したように、移動体１が移動中に、連続して特徴点の追従を行い、その移動量を積算していくことで経路を算出する構成となっている。
【０１３０】
そして、複数の特徴点から求められた移動体の移動量には、各特徴点毎に重みが付けられて、数１４式に示すよう加重平均を求めて最終的な移動量が算出されるが、本実施形態では、これまでの移動経路から予測された次の自己位置からのずれ量に応じた重みが用いられる。以下に詳述する。
【０１３１】
まず、演算工程は、これまでに複数の特徴点からの自己位置に基づいて総合的に算出した最終的な自己位置に関する値を、メインコンピュータ４５などに記憶しておき、後に移動体１の経路が参照できる。
【０１３２】
そして、演算工程は、自己位置の測定を複数回行った後に、以下の工程に示す動作を行う。すなわち、これまでの自己位置に関する値に基づいて装置自体の自己位置を予測して予測自己位置を算出する予測値算出工程と、複数の特徴点毎に計測した自己位置の各測定値と予測自己位置とを比較してそのずれ量をそれぞれ算出する比較工程と、複数の特徴点毎に計測した自己位置の各測定値に，各特徴点毎のずれ量に応じた重みを付けて最終的な自己位置を算出する最終自己位置算出工程と、である。なお、以下に説明するように、上記ずれ量が小さくにつれて各計測値に付ける重みが大きくなるよう設定される。
【０１３３】
上記予測値算出工程は、一定のサンプリング間隔で得られた自己移動量の時系列データから、次の瞬間の移動量を予測する。これは、移動体であるロボットの運動が一定の物理法則（運動方程式）に基づいており、極端に飛んだ値が計測されることはないという理由からである。具体的には、カルマンフィルタを用いて予測移動量を算出する。
【０１３４】
ここで、カルマンフィルタとは、あるシステムにおいて変化量の時系列データが測定できるときに、その変化の仕方をモデル化し、その変化量の測定値とシステムがどれだけの外乱を受けるか、というデータをあらかじめ与えておくことで、その変化量の予測・濾波・平滑を精度よく行うための手法である。但し、カルマンフィルタの詳細については省略し、以下、予測値の算出手法を説明する。
【０１３５】
カルマンフィルタを適用すべく、まず、システムを次式の基本システムで表すこととする。
【０１３６】
【数１７】

【０１３７】
【数１８】

【０１３８】
今、求めたい予測値は移動体の移動量であるので、これを（ｘ，ｙ，ｚ）として、その速度、加速度を、それぞれ（ｖ_ｘ，ｖ_ｙ，ｖ_ｚ）、（ａ_ｘ，ａ_ｙ，ａ_ｚ）とすると、数１９式のようになる。
【０１３９】
【数１９】

【０１４０】
そして、ｙ，ｚについても同様である。このとき、以下のように定義する。
【０１４１】
【数２０】

【０１４２】
【数２１】

【０１４３】
【数２２】

【０１４４】
【数２３】

【０１４５】
そして、上式のようにすると、カルマンフィルタのアルゴリズムから、以下の式を得られる。
【０１４６】
【数２４】

【０１４７】
となる。Ｋ_ｋはカルマンゲイン、Σ_ｘ０は、ｘ_０の共分散行列である。ここで、数２４式の最下段にある式の右辺は、一般に未知であるため、当該式は決定できない。そこで、数２４式の最上段式、及び、上から４番目の式の初期値として、以下の数２５式を採用する。
【０１４８】
【数２５】

但し、Ｏ_６は、６次元の零ベクトル、Ｉ_６×６は、６×６の単位行列を表す。
【０１４９】
【数２６】

【０１５０】
また、上式のように定義したのは、観測雑音の分散の不定性を、上式の右式の初期値の不定性の中にまとめて取り込むためである。ここで、雑音の分散比σ_ｗ ^２／σ_ｖ ^２が最終的に未知パラメータとしてカルマンフィルタに残る。
【０１５１】
以上より、システム雑音及び観測雑音の分散比σ_ｗ ^２，σ_ｖ ^２を決定すれば、移動量の予測値を含む状態予測ベクトルとして数２４式の上から２番目の式が、逐次得られる。なお、以下に説明する実施例では、上記分散比をトライアンドエラーにより決定する。
【０１５２】
以上のように、カルマンフィルタによって算出される移動量の予測値は、過去の移動量の時系列データをもとに算出されるため、計測開始時点で視界内に移動物体がなければ、算出される予測値にも移動物体の影響は入らない。そして、他の移動物体上の特徴点から算出した移動量は予測値から大きなずれが生じると考えられるため、複数得られた特徴点毎の移動量に対して予測値からのずれ量が大きいほど小さな荷重をかけ、一方、ずれ量が小さいほど大きな加重がかかるようなフィルタをかければ、他の移動物体の影響を抑制することができる。これにより、より制度の高い移動量を算出することができる。
【０１５３】
ここで、本実施形態では、統計の理論を応用し、予測値からのずれは、予測値を平均とする正規分布に従うと考え、実測値をもとに正規分布の分散σ^２を決定し、この正規分布関数の値を重みとして使用する。すなわち、各特徴点から計測された移動量が予測値と同一の場合に重みを１とし、当該予測値から離れるにつれて０に近づくよう、重みを設定する。なお、正規分布関数は、以下に説明するように、トライアンドエラーにより適切に設定し、かかる分布をメインコンピュータ４５や移動体１内のＣＰＵや所定の記憶部に記憶保持しておく。
【０１５４】
（実施例）
ここで、上述したように、カルマンフィルタを用いて移動体１の移動量の予測値を算出し、当該予測値に基づいた重みを使用して、複数の特徴点から自己位置を計測した実験を説明する。この実験では、図１乃至図２に示す自己位置計測システムが搭載された移動体１を前進走行させて、その移動体１のカメラ視界内で他の移動物体１００を移動させ、かかる環境下で移動体１の移動量を計測する。そして、上記カルマンフィルタの有効性を検証するというものである。
【０１５５】
ここで、かかる実験では、移動体１の本システムにより計測した移動量と比較する実際の移動量の計測（デッドレコニング）も必要になる。これについては、車輪の回転数とステア角をロータリエンコーダで測定して、これを積分して位置計測を行う手法であるオドメトリによる位置推定を行い、かかる計測値を理論値として取り扱う。なお、実験環境では、移動体１が前進走行する床は平面であり、滑りも起こりにくいため、直進移動距離の計測は誤差率１％以内である。
【０１５６】
また、実験で使用する他の移動物体１００は、自己位置を計測する移動体１とは異なる動きをするものであり、その構成の概要を図９に示す。この図に示すように、他の移動物体１００は、シャシ１０１に図示しない駆動源により駆動される自走用の車輪１０２を備え、また、移動体１が認識しやすい特徴点となりうる黒いフロックボール１０３を４つ高い位置に備えている。
【０１５７】
さらに、上述した数２４式の上から第４式に示すシステム雑音及び観測雑音の分散比σ_ｗ ^２，σ_ｖ ^２、及び、測定値と予測値とのずれの分散σ^２を決定する必要がある。ここで、σ_ｖ ^２については、あらかじめ測定でき、一方、σ_ｗ ^２についてはトライアンドエラーにより決定した。また、σ^２は、σ_ｗ ^２，σ_ｖ ^２を決定した後に測定した値をもとにトライアンドエラーを行って調節した。そして、これらの値は、各数式に代入されたり、あるいは、自己位置を計測する際に参照できるよう、メインコンピュータ４５や移動体１内のＣＰＵや記憶部などに記憶されている。
【０１５８】
このようにして、実際に実験を行ったときの様子を、図１０乃至図１３に示す。ここで、図１０（ａ）に示す例は、自己位置計測装置自体である移動体１を、壁２００に向かって静止させた状態で、他の移動物体１００を矢印Ａ１００に示すように、ラインＬ１００に沿って移動させる場合である。このとき、移動体１の前方には、種々の特徴点となりうる物体（図示せず）が存在しており、また、壁２００には、特徴点となる図形が描かれている。
【０１５９】
そして、図１０（ｂ）に、移動体１の自己位置の計測結果を示す。横軸には計測時間ｔ［ｓ（秒）］をとり、縦軸には移動体１の壁方向２００への移動距離ｔｘ［ｍｍ］をとる。この図に示すように、静止している移動体１において、その計測値はほぼ０の値を示している。すなわち、静止時には他の移動物体１００の影響をほぼ受けずに精度の高い自己位置の計測が可能である。なお、かかる実験においては、移動量として壁２００方向への距離ｘのみを計測しているが、これに限定されない。もちろん、上述してきたように、三次元（ｘ，ｙ，ｚ）における移動量も測定可能である。
【０１６０】
次に、図１１（ａ）に示すように、他の移動物体１００を矢印Ａ１００方向に、ラインＬ１００に沿って移動させ、また、移動体１を矢印Ａ１方向に前進させた場合を説明する。具体的には、自己位置を計測する移動体１が、他の移動物体１００に追い越される環境において計測を行った。そして、そのときの自己位置の計測結果を図１１（ｂ）に示す。この図では、図１０（ｂ）と同様に、横軸には計測時間ｔ［ｓ（秒）］をとり、縦軸には移動体１の移動距離ｔｘ［ｍｍ］をとっており、デッドレコニングによる移動量の理論値（実線）と、本システムによる移動量の計測値（菱形プロット）を示している。すると、当該計測値はほぼ理論値と差がなく、高精度な自己位置の計測が可能であることがわかる。
【０１６１】
さらに、図１２（ａ）に示すように、他の移動物体１００を矢印Ａ１０１方向に、ラインＬ１０１に沿って移動させ、また、移動体１を矢印Ａ１方向に前進させた場合を説明する。具体的には、自己位置を計測する移動体１が、他の移動物体１００とすれ違う環境において計測を行った。そして、そのときの自己位置の計測結果を図１２（ｂ）に示す。そして、この図には、デッドレコニングによる移動量の理論（実線）値と、本システムによる移動量の計測値（菱形プロット）とを示したが、このような実験例においても、当該計測値はほぼ理論値と差がなく、高精度な自己位置の計測が可能であることがわかる。
【０１６２】
以上のように、予測値に基づくフィルタを用いて自己位置の計測を行うことで、カメラの視界内に他の移動物体１００が存在している場合であっても、当該他の移動物体１００の存在の影響を抑制して、高精度な自己位置計測を実現できる。すなわち、他の移動物体１００を特徴点として追従した場合であっても、その影響を軽減して、自己位置を計測することができる。
【０１６３】
〈第８の実施形態〉
次に、本発明の第８の実施形態について説明する。本実施形態では、上述した構成の自己位置計測装置である移動体に、加速度センサが備えられている。そして、演算手段であるメインコンピュータ４５などが、移動体自体の加速度を計測し、また、撮影した画像中における他の物体の加速度を測定し、これらを比較することで、移動している他の物体を検出し、当該他の移動物体を特徴点から除く、という処理を行う機能を有している。例えば、移動体１の加速度との差が、あらかじめ定められた許容値よりも大きい場合には、画像中の移動物体上の所定領域を特徴点抽出領域から除くこととする。
【０１６４】
これにより、取得画像中の他の移動物体が特徴点となることを抑制して、位置計測の精度向上を図ることができると共に、そのような移動物体が存在する場所においても、本発明である移動体にて自己位置を計測することができ、汎用性の向上を図ることができる。
【０１６５】
〈第９の実施形態〉
次に、本発明の第９の実施形態について説明する。当該第９の実施形態においては、自己位置計測装置は人間が常時身につける携帯型である。例えば、登山者の帽子に装着可能な装置であって、かかる装置にステレオカメラ、傾斜角センサ、ＣＰＵなどが備えられている。そして、ステレオカメラなどで取得したデータは、無線通信手段を介して遠隔地のコンピュータに送信され、かかるコンピュータにて処理されてもよい。
【０１６６】
このようにすることにより、移動する人間の移動経路を計測したり、現在の位置を他の者が把握することが容易となる。そして、かかる測定値は、例えば、登山者が遭難したときの救助に利用することができる。
【０１６７】
【発明の効果】
本発明は、上記のように構成され機能するので、これによると、移動前後における周囲の少なくとも一つの特徴点の相対位置座標と、自己の姿勢角の変化情報とを測定し、これらをあらかじめ定められた式に代入して演算することにより、移動前の位置に対する移動後の自己位置を計測することができるため、安定かつ高精度な移動経路計測及び位置計測を行うことができる、という従来にない優れた効果を有する。特に、従来の少なくとも４つの特徴点から自己位置を計測する方法に対しては、安定性の面でその効果が顕著である。
【０１６８】
また、特徴点を複数抽出して、それぞれの特徴点について自己位置の計測を行い、それらの統計から総合的に自己位置を決定するという構成とすることで、さらなる計測値の精度向上を図ることができる。
【０１６９】
さらに、複数の特徴点から求めた自己位置にそれぞれ重みを付けて最終的な自己位置を決定する構成とすると共に、信頼性が高いと考えられる特徴点からの自己位置に付ける重みを大きく設定することで、周囲に他の移動物体が存在した場合であっても、かかる物体の影響を抑制することができ、より精度の高い自己位置の計測を実現できる。
【図面の簡単な説明】
【図１】本発明の第１の実施形態における動作の様子を示す概略図である。図１（ａ）は、時刻ｔの時の様子を示す図であり、図１（ｂ）は、時刻ｔ＋１の時の様子を示す図である。
【図２】本発明の第１の実施形態における構成を示すブロック図である。
【図３】本発明の第１の実施形態における自己位置を計測する手順の概略を示すフローチャートである。
【図４】第１の実施形態における画像処理時の動作を示すフローチャートである。
【図５】図５（ａ）〜（ｈ）は、第１の実施形態における画像の処理状態を示す説明図である。
【図６】図６（ａ）〜（ｄ）は、第１の実施形態において、特徴点を抽出する際の処理状態を示す説明図である。
【図７】本発明の第２の実施形態における動作の様子を示す概略図である。図２（ａ）は、時刻ｔの時の様子を示す図であり、図２（ｂ）は、時刻ｔ＋１の時の様子を示す図である。
【図８】図８（ａ）〜（ｄ）は、本発明の第３の実施形態における画像処理領域の具体例を示す説明図である。
【図９】本発明の第１０の実施形態において用いる他の移動物体を示す概略図である。
【図１０】本発明の第１０の実施形態における計測実験の一例の様子を示す図である。図１０（ａ）は、実験の様子を示す説明図であり、図１０（ｂ）は、計測結果を示す図である。
【図１１】本発明の第１１の実施形態における計測実験の一例の様子を示す図である。図１１（ａ）は、実験の様子を示す説明図であり、図１１（ｂ）は、計測結果を示す図である。
【図１２】本発明の第１２の実施形態における計測実験の一例の様子を示す図である。図１２（ａ）は、実験の様子を示す説明図であり、図１２（ｂ）は、計測結果を示す図である。
【符号の説明】
１移動体
２ステレオカメラ（位置測定手段）
３姿勢角センサ（姿勢検出手段）
１０メイン処理部
４１画像処理部（位置測定手段、演算手段）
４２ＣＰＵ（演算手段）
４５メインコンピュータ（演算手段）

Claims

装置自体の移動前に対する移動後の自己位置を計測する方法であって、
移動前に，装置自体に備えられた位置測定手段にて周囲の少なくとも一つの特徴点の相対位置を測定すると共に，装置自体に備えられた姿勢検出手段にて装置自身の姿勢情報を検出する移動前工程と、
移動後に，前記位置測定手段にて前記特徴点の相対位置を測定すると共に，前記姿勢検出手段にて装置自体の姿勢情報を検出する移動後工程と、
装置自体に備えられた演算手段を用いて，前記検出した姿勢情報から装置自身の移動前後における姿勢変化量である姿勢変化情報を算出すると共に，当該姿勢変化情報と装置の移動前後における前記特徴点の各相対位置情報とに基づいて装置自体の移動前に対する移動後の自己位置を算出する演算工程と、
を備えたことを特徴とする自己位置計測方法。
前記移動前工程は、前記位置測定手段にてあらかじめ定められた判断基準に基づいて周囲の任意の箇所を特徴点として選定することを特徴とする請求項１記載の自己位置計測方法。
前記移動前工程は、前記位置測定手段にてあらかじめ定められた判断基準に基づいて一つの特徴点を選定すると共に、
前記移動後工程は、前記移動前工程にて選定した特徴点と同一の特徴点を抽出する、ことを特徴とする請求項２記載の自己位置計測方法。
前記移動前工程は、あらかじめ定められた判断基準に基づいて複数の任意の特徴点を選定すると共に，それぞれの特徴点の相対位置を測定し、
前記移動後工程は、前記移動前工程にて選定した複数の特徴点と同一の特徴点をそれぞれ抽出して各特徴点の相対位置を測定し、
前記演算工程は、前記複数の特徴点毎に装置自体の移動前に対する移動後の自己位置を計測して，これら複数の計測値に基づいて最終的な自己位置を算出することを特徴とする請求項２記載の自己位置計測方法。
前記移動前工程及び前記移動後工程は、ステレオ視可能な複数のカメラによって構成された前記位置測定手段を用いて，前記各カメラによる複数の取得画像をマッチングすることにより各画像毎にそれぞれ対応する前記各特徴点を検出し，ステレオ視の原理により当該各特徴点の相対位置を測定すると共に、
前記移動後工程は、前記各カメラにて取得した複数の画像をマッチングする際に算出する当該マッチングの一致度に関する値を，前記各特徴点毎に対応付けて前記位置測定手段あるいは前記演算手段に記憶し、
前記演算工程は、前記複数の特徴点毎に計測した装置自体の移動前後における自己位置の各計測値に，前記各特徴点の一致度に関する値に応じた重みを付けて最終的な自己位置を算出する、ことを特徴とする請求項４記載の自己位置測定方法。
前記演算工程は、前記一致度が高くなるにつれて前記各計測値に付ける重みが大きくなるよう設定する、ことを特徴とする請求項５記載の自己位置測定方法。
前記移動前工程及び前記移動後工程は、画像取得手段にて構成された前記位置測定手段を用いて，取得画像の濃淡値に基づいて前記複数の特徴点を選定及び検出し，当該各特徴点の相対位置を測定すると共に、
前記移動前工程は、前記各特徴点を選定する際に算出する当該各特徴点の濃淡値に関する情報である濃淡特徴量を，当該各特徴点に対応付けて前記位置測定手段あるいは前記演算手段に記憶し、
前記演算工程は、前記複数の特徴点毎に計測した装置自体の移動前後における自己位置の各測定値に，前記各特徴点の濃淡特徴量に応じた重みを付けて最終的な自己位置を算出する、ことを特徴とする請求項４記載の自己位置測定方法。
前記移動前工程は、前記濃淡特徴量を前記画像上の所定領域内における最大濃淡値と最小濃淡値との差で表すと共に、
前記演算工程は、前記濃淡特徴量が大きくなるにつれて前記各計測値に付ける重みが大きくなるよう設定する、ことを特徴とする請求項７記載の自己位置測定方法。
前記移動後工程及び前記演算工程は、前記演算工程によって装置自体の移動前後における自己位置が算出された後に繰り返し作動すると共に，その際に，直前に作動した前記移動後工程にて抽出された前記複数の特徴点を前記移動前工程にて選定した複数の特徴点とみなして作動し、
前記移動後工程は、前記各特徴点を抽出する際に，当該各特徴点を連続して検出した回数をカウントすると共に，当該回数の値を前記各特徴点毎に前記位置測定手段あるいは前記演算手段に記憶し、
前記演算工程は、前記自己位置の測定を複数回行った後に，前記複数の特徴点毎に計測した装置自体の移動前後における自己位置の各測定値に，前記各特徴点の検出回数に応じた重みを付けて最終的な自己位置を算出する、ことを特徴とする請求項４記載の自己位置測定方法。
前記演算工程は、前記検出回数が多くなるにつれて前記各計測値に付ける重みが大きくなるよう設定する、ことを特徴とする請求項９記載の自己位置測定方法。
前記演算工程は、算出した装置自体の移動前後における最終的な自己位置に関する値を前記演算手段に記憶し、
前記移動後工程及び前記演算工程は、前記演算工程によって装置自体の移動前後における自己位置が算出された後に繰り返し作動すると共に，その際に，直前に作動した前記移動後工程にて抽出された前記複数の特徴点を前記移動前工程にて選定した複数の特徴点とみなして作動し、
前記演算工程は、自己位置の測定を複数回行った後に、
これまでの自己位置に関する値に基づいて装置自体の移動後の自己位置を予測して予測自己位置を算出する予測値算出工程と、
前記複数の特徴点毎に計測した自己位置の各測定値と前記予測自己位置とを比較して，当該各特徴点毎にそのずれ量を算出する比較工程と、
前記複数の特徴点毎に計測した自己位置の各測定値に，前記各特徴点毎に算出した前記ずれ量に応じた重みを付けて最終的な自己位置を算出する最終自己位置算出工程と、
を備えたことを特徴とする請求項４記載の自己位置測定方法。
前記演算工程は、前記ずれ量が小さくなるにつれて前記各計測値に付ける重みが大きくなるよう設定する、ことを特徴とする請求項１１記載の自己位置測定方法。
装置自体の移動前に対する移動後の自己位置を計測する装置であって、
装置周囲の少なくとも一つの特徴点の相対位置を測定する位置測定手段と、装置自身の姿勢情報を検出する姿勢検出手段と、これらの動作を制御して取得した情報を演算処理する演算手段とを備え、
前記位置測定手段が、装置の移動前後におけるそれぞれの前記特徴点の相対位置を測定すると共に、前記姿勢検出手段が、装置自体の移動前後におけるそれぞれの姿勢情報を取得し、
前記演算手段が、前記取得した姿勢情報の移動前後における変化量である姿勢変化情報を算出すると共に、当該姿勢変化情報と装置自体の移動前後における前記特徴点の各相対位置情報とに基づいて当該装置自体の移動前に対する移動後の自己位置を算出する機能を有することを特徴とする自己位置計測装置。
装置自体の周囲画像を取得する画像取得手段と、装置自身の姿勢情報を検出する姿勢検出手段と、前記画像及び検出した姿勢情報を処理する演算手段とを備え、装置自体の移動前の対する移動後の自己位置を計測する装置であって、
前記演算手段が、前記画像取得手段にて取得した画像を用いて周囲画像中の少なくとも一つの特徴点の相対位置を当該装置自身の移動前後においてそれぞれ測定する特徴点位置測定機能と、前記姿勢検出手段にて検出した当該装置自身の移動前後における姿勢情報からその変化量を算出する姿勢変化情報算出機能と、装置自体の移動前後の前記特徴点の各位置情報と前記姿勢情報変化量とを予め定められた式に代入することにより装置自体の移動量を算出する移動量算出機能と、を有し、
これにより装置自体の移動前に対する移動後の自己位置を計測することを特徴とする自己位置計測装置。