JP2004294421A - 自己位置計測方法及び装置 - Google Patents
自己位置計測方法及び装置 Download PDFInfo
- Publication number
- JP2004294421A JP2004294421A JP2003144065A JP2003144065A JP2004294421A JP 2004294421 A JP2004294421 A JP 2004294421A JP 2003144065 A JP2003144065 A JP 2003144065A JP 2003144065 A JP2003144065 A JP 2003144065A JP 2004294421 A JP2004294421 A JP 2004294421A
- Authority
- JP
- Japan
- Prior art keywords
- movement
- self
- feature points
- feature
- feature point
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Landscapes
- Length Measuring Devices By Optical Means (AREA)
- Image Processing (AREA)
- Image Analysis (AREA)
Abstract
【解決手段】移動前に、装置1自体に備えられた位置測定手段2,45にて周囲の少なくとも一つの特徴点の相対位置を測定すると共に、装置1自体に備えられた姿勢検出手段3にて装置自身の姿勢情報を検出する移動前工程と、移動後に、位置測定手段2,41にて特徴点の相対位置を測定すると共に、姿勢検出手段3にて装置自体の姿勢情報を検出する移動後工程と、装置自体に備えられた演算手段45等を用いて、検出した姿勢情報から装置自身の移動前後における姿勢変化量である姿勢変化情報を算出すると共に、当該姿勢変化情報と特徴点の移動前後の各相対位置情報とに基づいて装置自体の移動前に対する移動後の自己位置を算出する演算工程とを備えた。
【選択図】 図1
Description
【発明の属する技術分野】
本発明は、自己位置計測方法及び装置にかかり、特に、移動体や持ち運び可能な装置自体の移動前の位置に対する移動後の自己位置を計測する方法及び装置に関する。
【0002】
【従来の技術】
従来より、装置自身の自己位置を測定する方法が研究され、それを実現する装置が開発されている。そして、移動体自体や人間等が持ち歩く物体に応用し、その移動後の位置を計測して、当該位置を繋ぎ合わせることにより、一定時間経過後の装置の移動経路や現在位置を計測することができる。
【0003】
ここで、従来より用いられている自己位置測定方法として、画像を用いるものがある。そのような画像のみによって自己位置を測定するシステムは、環境上の少なくとも4つの地点の相対位置(あるいは相対位置の変化量)を常に正確にかつ安定に測定し続ける必要がある。しかし、4地点の位置すべてを常に安定に計測し続けることは、実際には非常に困難なことであり、一瞬でも3点以下になるとその時点で機能しなくなってしまう。仮に、4点の計測が出来たとしても、そのうち一点でも計測精度が低いと計測位置の精度が甚だしく劣化するという問題がある。
【0004】
また、この手法では、4地点の座標から三角測量で自己位置を求めるので、必然的にその4地点は互いに離れているということとなる。従って、その4点は画面の周辺部付近に写っている場合には、カメラが移動すればその4点のうちいずれかはすぐに画面上から外れることとなる。これは4点を継続的に測定し続けることが構造的に困難であることを意味している。
【0005】
この問題を克服するためには、複数のカメラを用いるか、あるいは、特殊な広角のレンズを用いる必要があった。但し、複数のカメラセットを用いればシステムが複雑になる上、カメラ間の精度を高く維持することが困難となる。また、広角のレンズを用いると精度が落ちる等の問題が生じる。また、別の問題として、計測画像中に移動物体がある場合は使用が困難であった。
【0006】
そして、上記従来技術の問題点を解決すべく、特許文献1に示すような技術も開示されている。この特許文献1に示す技術は、走行中に周辺画像を撮影し、走行周辺環境の画像により自己位置を測定して目標に向かって移動するというものであり、自走車に搭載された自己位置測定装置である。この装置においては、実空間での位置が既知である複数の特徴物を画像上で検出し、それぞれの特徴物の実空間における位置と画像面における位置とに基づいて、自己位置を求めている。
【0007】
【特許文献1】
特開平9−53939号公報
【0008】
【発明が解決しようとする課題】
しかしながら、上記特許文献1記載の従来技術にも以下のような不都合があった。すなわち、特許文献1のものは、特徴物の実空間での位置をあらかじめ装置に記憶しておかなければならず、特定の場所以外では、自己位置の計測ができないと問題があった。
【0009】
また、上述したように、画像上において4点の特徴点を抽出する方法は自己位置計測の精度を維持することが困難であり、いずれにしても、自己位置を計測する従来技術には種々の問題が生じている。
【0010】
【発明の目的】
本発明は、上記従来例の有する不都合を改善し、特に、あらゆる場所において、安定して精度よく自己位置を計測することができる方法及び装置を提供することをその目的とする。
【0011】
【課題を解決するための手段】
そこで、本発明では、装置自体の移動前に対する移動後の自己位置を計測する方法であって、移動前に、装置自体に備えられた位置測定手段にて周囲の少なくとも一つの特徴点の相対位置を測定すると共に、装置自体に備えられた姿勢検出手段にて装置自身の姿勢情報を検出する移動前工程と、移動後に、位置測定手段にて特徴点の相対位置を測定すると共に、姿勢検出手段にて装置自体の姿勢情報を検出する移動後工程とを備えている。そして、移動後工程に続いて、装置自体に備えられた演算手段を用いて、検出した姿勢情報から装置自身の移動前後における姿勢変化量である姿勢変化情報を算出すると共に、当該姿勢変化情報と特徴点の移動前後の各相対位置情報とに基づいて装置自体の移動前に対する移動後の自己位置を算出する演算工程を備えた、という構成を採っている。
【0012】
このような方法による構成を採ることにより、まず、位置測定手段にて移動前に少なくとも一つの特徴点の装置に対する相対位置を測定すると共に、姿勢検出手段にて装置自体の姿勢情報を検出する。そして、移動後にも、同一の特徴点の相対位置を測定して、装置自体の姿勢情報を検出する。その後、演算手段にて、移動前後の特徴点の各相対位置情報と自己の姿勢変化情報とを、あらかじめ定められた式に代入して演算することにより、装置自体の移動前の位置に対する移動後の自己位置を算出する。従って、少なくとも一つの特徴点の位置情報と、姿勢情報とを測定することで自己位置を測定することができるため、簡易かつ安定して位置を計測することができ、計測精度の向上をも図ることができる。
【0013】
また、移動前工程は、位置測定手段にてあらかじめ定められた判断基準に基づいて周囲の任意の箇所を特徴点として選定する、という構成であると望ましい。これにより、あらゆる環境においても任意の特徴点を選定することで自己位置を計測することができ、適応性の向上を図ることができる。
【0014】
また、移動前工程は、位置測定手段にてあらかじめ定められた判断基準に基づいて一つの特徴点を選定すると共に、移動後工程は、移動前工程にて選定した特徴点と同一の特徴点を抽出する、という構成であってもよい。これにより、上述したように一つの特徴点でも自己位置を計測することができるため、当該計測を実現しつつ、計測処理の情報量の減少を図り、演算手段の処理負担を抑制することができる。また、計測処理の迅速化を図ることができ、自己位置計測の実時間処理を実現する。
【0015】
さらに、移動前工程は、あらかじめ定められた判断基準に基づいて複数の任意の特徴点を選定すると共に、それぞれの特徴点についての位置を測定し、移動後工程は、移動前工程にて選定した複数の特徴点と同一の特徴点をそれぞれ抽出して各特徴点についての位置を測定し、演算工程は、複数の特徴点毎に装置自体の移動前に対する移動後の自己位置を計測して、これら複数の計測値に基づいて最終的な自己位置を算出する、という構成とするとなお望ましい。
【0016】
これにより、特徴点を複数選定し、各特徴点毎に自己位置を計測してそれら計測結果を総合して最終的な自己位置を計測することで、さらなる計測精度の向上を図ることができる。また、複数の特徴点を選定することにより、移動後に抽出できない特徴点が生じたとしても、他の特徴点から自己位置を計測することができるため、安定して計測を行うことができる。
【0017】
また、上記複数の特徴点を用いて自己位置を測定する方法において、移動前工程及び移動後工程は、ステレオ視可能な複数のカメラによって構成された位置測定手段を用いて,各カメラによる複数の取得画像をマッチングすることにより各画像毎にそれぞれ対応する各特徴点を検出し,ステレオ視の原理により当該各特徴点の相対位置を測定すると共に、移動後工程は、各カメラにて取得した複数の画像をマッチングする際に算出する当該マッチングの一致度に関する値を,各特徴点毎に対応付けて位置測定手段あるいは演算手段に記憶し、演算工程は、複数の特徴点毎に計測した装置自体の移動前後における自己位置の各計測値に,各特徴点の一致度に関する値に応じた重みを付けて最終的な自己位置を算出する、という構成であると望ましい。このとき、演算工程は、マッチング時の一致度が高くなるにつれて各計測値に付ける重みが大きくなるよう設定する、こととするとなお望ましい。
【0018】
このようにすることで、各特徴点毎に複数の得られた自己位置の測定値を総合して最終的な自己位置を算出する際に、精度よく検出できた特徴点から算出された装置の自己位置の計測値に大きな重みを付加し、当該計測値をより重視して最終的な自己位置を計測することとなる。従って、より信頼性の高い自己位置を算出することができ、計測制度の向上を図ることができる。
【0019】
また、上記同様に複数の特徴点を用いて測定する場合に、移動前工程及び移動後工程は、画像取得手段にて構成された位置測定手段を用いて,取得画像の濃淡値に基づいて複数の特徴点を選定及び検出し,当該各特徴点の相対位置を測定すると共に、移動前工程は、各特徴点を選定する際に算出する当該各特徴点の濃淡値に関する情報である濃淡特徴量を,当該各特徴点に対応付けて位置測定手段あるいは演算手段に記憶し、演算工程は、複数の特徴点毎に計測した装置自体の移動前後における自己位置の各測定値に,各特徴点の濃淡特徴量に応じた重みを付けて最終的な自己位置を算出する、という構成であると望ましい。このとき、移動前工程は、濃淡特徴量を画像上の所定領域内における最大濃淡値と最小濃淡値との差で表すと共に、演算工程は、濃淡特徴量が大きくなるにつれて各計測値に付ける重みが大きくなるよう設定する、こととするとなお望ましい。
【0020】
このようにすることで、濃淡差の大きい、より特徴のある特徴点から算出された装置の自己位置の計測値に大きな重みを付加し、当該計測値をより重視して最終的な自己位置を計測することとなる。従って、より信頼性の高い自己位置を算出することができ、計測制度の向上を図ることができる。
【0021】
また、上記同様に複数の特徴点を用いて測定する場合に、移動後工程及び演算工程は、演算工程によって装置自体の移動前後における自己位置が算出された後に繰り返し作動すると共に,その際に,直前に作動した移動後工程にて抽出された複数の特徴点を移動前工程にて選定した複数の特徴点とみなして作動し、移動後工程は、各特徴点を抽出する際に,当該各特徴点を連続して検出した回数をカウントすると共に,当該回数の値を各特徴点毎に位置測定手段あるい演算手段に記憶し、演算工程は、自己位置の測定を複数回行った後に,複数の特徴点毎に計測した装置自体の移動前後における自己位置の各測定値に,各特徴点の検出回数に応じた重みを付けて最終的な自己位置を算出する、という構成であると望ましい。このとき、演算工程は、検出回数が多くなるにつれて各計測値に付ける重みが大きくなるよう設定する、こととするとより望ましい。
【0022】
このようにすることで、継続して特徴点の追従を行い、装置の移動経路をも計測する構成において、連続して検出される信頼性の高い特徴点から算出された装置の自己位置の計測値に大きな重みを付加し、当該計測値をより重視して最終的な自己位置を計測することとなる。従って、より信頼性の高い自己位置を算出することができ、計測制度の向上を図ることができる。
【0023】
さらに、上記同様に複数の特徴点を用いて測定し、また、継続して特徴点を追従して装置の移動経路をも計測する構成において、演算工程は、算出した装置自体の移動前後における最終的な自己位置に関する値を演算手段に記憶し、演算工程は、自己位置の測定を複数回行った後に、これまでの自己位置に関する値に基づいて装置自体の移動後の自己位置を予測して予測自己位置を算出する予測値算出工程と、複数の特徴点毎に計測した自己位置の各測定値と予測自己位置とを比較して,当該各特徴点毎にそのずれ量を算出する比較工程と、複数の特徴点毎に計測した自己位置の各測定値に,各特徴点毎に算出したずれ量に応じた重みを付けて最終的な自己位置を算出する最終自己位置算出工程と、を備えた構成であると望ましい。このとき、演算工程は、ずれ量が小さくなるにつれて各計測値に付ける重みが大きくなるよう設定する、こととすると望ましい。
【0024】
このようにすることで、まず、過去の移動経路に基づいてあらかじめ定められた演算手法にて算出された予測値が算出される。そして、当該予測値と特徴点毎に算出された自己位置とが比較され、その差であるずれ量の少ない自己位置の計測値に大きな重みを付加し、当該計測値をより重視して最終的な自己位置を計測する。従って、ずれ量が大きく、次の自己位置として可能性の低い値の影響を抑制し、より信頼性の高い自己位置を算出することができ、計測制度の向上を図ることができる。
【0025】
また、本発明では、装置自体の移動前に対する移動後の自己位置を計測する装置であって、装置周囲の少なくとも一つの特徴点の相対位置を測定する位置測定手段と、装置自身の姿勢情報を検出する姿勢検出手段と、これらの動作を制御して取得した情報を演算処理する演算手段とを備え、位置測定手段が、装置の移動前後におけるそれぞれの特徴点の相対位置を測定すると共に、姿勢検出手段が、装置自体の移動前後におけるそれぞれの姿勢情報を取得し、演算手段が、取得した姿勢情報の移動前後における変化量である姿勢変化情報を算出すると共に、当該姿勢変化情報と装置自体の移動前後における特徴点の各相対位置情報とに基づいて当該装置自体の移動前に対する移動後の自己位置を算出する機能を有する、という構成の自己位置計測装置をも提供している。
【0026】
さらに、具体的には、装置自体の周囲画像を取得する画像取得手段と、装置自身の姿勢情報を検出する姿勢検出手段と、画像及び検出した姿勢情報を処理する演算手段とを備え、装置自体の移動前の対する移動後の自己位置を計測する装置であって、演算手段が、画像取得手段にて取得した画像を用いて周囲画像中の少なくとも一つの特徴点の相対位置を当該装置自身の移動前後においてそれぞれ測定する特徴点位置測定機能と、姿勢検出手段にて検出した当該装置自身の移動前後における姿勢情報からその変化量を算出する姿勢変化情報算出機能と、装置自体の移動前後の特徴点の各位置情報と姿勢情報変化量とを予め定められた式に代入することにより装置自体の移動量を算出する移動量算出機能と、を有し、これにより装置自体の移動前に対する移動後の自己位置を計測する、という自己位置計測装置をも提供している。
【0027】
このような構成を採っても、上述同様の作用、効果を得ることができ、上記目的を達成することができる。
【0028】
【発明の実施の形態】
〈第1の実施形態〉
以下、本発明の第1の実施形態について、図1乃至図6を参照して説明する。図1は、本実施形態における自己位置計測装置の動作の様子を示す概略図である。図2は、自己位置計測装置の構成を示すブロック図である。図3は、装置の自己位置を計測する手順を示すフローチャートである。図4は、装置にて画像処理をする時の動作を示すフローチャートである。図5乃至図6は、画像処理の様子を示す説明図である。
【0029】
(全体構成)
本発明である自己位置計測装置は、本実施形態においては車両型の移動体1に備えられている。そして、移動体1自体の移動前の位置に対する移動後の自己位置を計測するものである。その構成の概略を、まず図1(a)を参照して説明する。
【0030】
図1(a)に示す移動体1は、移動体1周囲の少なくとも一つの特徴点の当該移動体1に対する相対位置を測定する位置測定手段2と、移動体自身の姿勢情報を検出する姿勢検出手段(図示せず)と、これらの動作を制御して取得した情報を演算処理する演算手段(図示せず)とを備えている。これら各構成要素は、概ね以下の機能を有する。
【0031】
位置測定手段2は、移動体1の移動前後において特徴点を抽出、追跡(トラッキング)して、当該特徴点の装置に対する相対位置を測定する。姿勢検出手段は、移動体自体の移動前後におけるそれぞれの姿勢情報を取得する。そして、演算手段は、取得した姿勢情報の移動前後における変化量である姿勢変化情報を算出すると共に、当該姿勢変化情報と移動体自体の移動前後における特徴点の各相対位置情報とに基づいて当該移動体自体の移動前に対する移動後の自己位置を算出する。
【0032】
ここで、本実施形態では、具体的には図2に示すような構成を採っている。まず、位置測定手段2は、移動体1自体の周囲画像を取得する画像取得手段であるステレオカメラである。このステレオカメラ2は、図1(a)に示すように、移動体1のシャシ部11から上方に延びる支柱12の上端部に固定されて備えられている。そして、本実施形態では、ステレオカメラ2は2つのCCDカメラにて構成されており、一方のカメラから取り込まれた画像をマスター画像、他方のカメラにて取り込まれた画像をスレーブ画像として取り扱う。なお、ステレオカメラは2つのカメラにて構成されることに限定されない。
【0033】
また、移動体1自身の姿勢情報を検出する姿勢検出手段は、当該移動体1の姿勢角を検出する姿勢角センサ3である。この姿勢角センサ3は、図1(a)では図示しないが、移動体1のシャシ部11に固定されて備えられている。さらに、ステレオカメラ2にて取り込んだ画像、及び、姿勢角センサ3にて検出した姿勢情報を処理する演算手段である演算処理部41,42,43,44,45を、移動体1の内外に備えている。以下、これをさらに詳述する。
【0034】
(移動体)
まず、移動体1の構成の概略を図1(a)に示すが、この移動体1は、シャシ部11の左右にホイール13,14(2輪)を備えていて、これにより走行する車両である。例えば、月面などを探査する無人移動体である。但し、移動体1の構成はこれに限定されることなく、例えば、一般的な有人の自動車などの移動体であってもよい。
【0035】
そして、この移動体1を走行させるべく、各ホイール13a,13bを駆動するDCモータ13a,14a、当該各モータの回転角度を検出するエンコーダ13b、14b、各エンコーダから検出値を読み取って上位コンピュータからの指令に基づいてDCモータ13a,14aに回転制御指令を発するモータドライバとしてのCPU43,44が、シャシ部11に備えられている。
【0036】
(ステレオカメラ)
ステレオカメラ2は、上述したように移動体1のシャシ部11の上部に備えられていて、移動体周囲のステレオ視差画像を取り込む。取り込んだ画像は、演算手段としての画像処理部41などにて処理され、その一部はトランスミッター15を介して遠隔地にあるメイン処理部10のモニタ10bに送信され、オペレータに表示される。また、画像処理部41は、周囲画像からあらかじめ定められたアルゴリズムに基づくプログラムが作動することにより特徴点を抽出し、かかる特徴点の位置を表す三次元座標(ボディ座標系)を算出する機能を有する。すなわち、画像に関する演算手段としての画像処理部41は、画像取得手段にて取得した画像を用いて周囲画像中の少なくとも一つの特徴点の相対位置を当該移動体1自身の移動前後においてそれぞれ測定する特徴点位置測定機能を備えている。その具体的な処理については動作説明時に詳述する。なお、ステレオカメラ2である画像取得手段と、取得した画像を処理する画像処理部41等(メインコンピュータ45を含む場合あり)とによる構成を、上記では位置測定手段と呼んでいる。
【0037】
そして、画像から測定された特徴点の位置情報は、ハブ16、無線LAN17,10aを介して遠隔地に位置するメイン処理部10のメインコンピュータ45に送信され、移動体1自身の自己位置を算出する際に用いられる。メイン処理部10については後述する。
【0038】
但し、上記特徴点の位置情報は、ハブ16を介して移動体1に搭載された別のCPU42に送信され、当該CPU42にて移動体1の自己位置が算出されてもよい。また、画像から特徴点を抽出する処理などの一連の処理が、遠隔地に位置するメイン処理部10のメインコンピュータ45にて実行されてもよい。すなわち、取得した画像情報自体を、ハブ16から無線LAN17,10aを介してメインコンピュータ45に送信してもよい。このようにしても、メインコンピュータ45が遠隔地にあり、処理するコンピュータが異なるのみであって、本発明の構成としては同一である。
【0039】
(姿勢角センサ)
姿勢角センサ3は、移動体1のシャシ部11に固定して備えられており、本実施形態では、傾斜角センサと方位計を姿勢角センサ3として用いることで、シャシ部11の向き、傾斜情報を取得する。そして、これらの情報はCPU42が姿勢角センサ3の動作を制御することで当該姿勢角センサ3から取得し、その後は、ハブ16、無線LAN17,10aを介してメインコンピュータ45に送信され、当該コンピュータ45にて自己位置の算出処理に用いられる。但し、傾斜角などの姿勢角に関する情報を検出したCPU42が、後述するメインコンピュータ45にて行われる処理すなわち前回検出した値と比較してその変化量を算出する処理を実行してもよい。その処理については後述する。
【0040】
ちなみに、姿勢角センサ3として、上記傾斜角センサや方位計を用いることで、比較的安価に本発明にかかる装置を構成することができる。但し、姿勢角センサ3はこれに限定されるものではない。センサ3から検出した姿勢角情報からその変化量がわかればよく、必ずしも移動体1の絶対角を検出する必要がないため、ジャイロスコープなどを用いてもよい。具体的には、3軸のジャイロ値を積分してもよい。また、姿勢角が変化しないことが明確な場合、すなわち、水平面上を移動する場合には、方位角の1軸のみ計測すればよい。
【0041】
ここで、上記CPU42は、メインコンピュータ45からの指令に応じて、左右のホイール13,14の駆動状態を制御する他のCPU43,44に駆動指令を発する機能も有する。
【0042】
(メイン処理部)
また、本実施形態では、無人の移動体1であるため、オペレータによる走行指令が入力され、当該走行指令に応じて移動体11の走行状態を制御するメイン処理部10が、移動体1と分離して設けられている。ここで、移動体1が自走式である場合には、自走するための制御プログラムを実行して、遠隔にて移動体1の走行状態を制御してもよい。但し、メイン処理部10は移動体1自体に備えられていてもよい。
【0043】
また、メイン処理部10には、CPUやメモリなどを有するメインコンピュータ45が搭載されており、無線LAN10aといった通信手段を介して移動体1とデータ通信を行う。さらに、メイン処理部10には、移動体1にて取り込まれ送信された画像を受信して表示するモニタ10bが備えられている。これにより、オペレータは、移動体1の周囲の状況をモニタ10bにて確認しながら走行指令を出すことができる。
【0044】
ここで、メインコンピュータ45は、主に、姿勢角センサ3にて検出し移動体1から送信された姿勢情報から、当該移動体1自身の移動前後における変化量である姿勢角変化情報を算出する姿勢変化情報算出機能と、移動体1自体の移動前後の特徴点の各位置情報と姿勢情報変化量とを予め定められた式に代入することにより当該移動体1自体の移動量を算出する移動量算出機能とを備えている。
【0045】
具体的には、姿勢角変化情報とは、姿勢角センサ3にて検出したシャシ部11の向き、傾斜角から、ロール角、ピッチ角、ヨー角の変化量を算出する。そして、移動量算出機能にて、当該ロール角等の情報と、移動前後における特徴点の各位置情報とを用いて、あらかじめメインコンピュータ45に記憶されている算出式に代入することで、移動体1の移動前に対する移動後の三次元座標を得ることができる。
【0046】
そして、算出した移動体1の移動量に関する情報を当該メインコンピュータ45が記憶し、その後の移動量も蓄積することにより、移動体1の移動経路や現在位置を算出することができる。
【0047】
ここで、メインコンピュータ45は、上述した移動体1上の演算手段41,42,43,44が実行する処理を全て行ってもよい。いずれにしても、移動体1上の演算手段41等で処理しようと、遠隔地にあるメインコンピュータ45で処理しようとも、その構成に相違はない。
【0048】
(動作)
次に、本発明の第1の実施形態における基本的な動作を、図3を参照して説明する。図3は、本実施形態における基本的な動作を示すフローチャートである。なお、以下に示す動作は、メインコンピュータ45や移動体1内の各処理部(画像処理部41やCPU42,43,44など)に、あらかじめ各動作用プログラムが組み込まれることで種々の機能が構築されて実現される。また、以下の動作を実現するために必要なデータも、あらかじめメインコンピュータ45などに記憶されている。
【0049】
移動体1は、まず移動を開始する前、あるいは、移動中のあるタイミングにて、ステレオカメラ2にて周囲画像を取得する。そして、画像処理部41にて取り込んだ画像を処理し、当該周囲画像中から少なくとも一つの特徴点(本実施形態では1つの特徴点)を抽出する。その後、移動体1から抽出した特徴点までの相対位置を測定し、三次元座標を算出する。また、これと共に、姿勢角センサ3にて移動体1自体の向き、傾斜角などの姿勢情報を検出する。そして、これらの情報は、移動体1からメイン処理部10へと送信され、メインコンピュータ45に記憶保持される(ステップS1、移動前工程)。
【0050】
続いて、移動体1が移動し、あるいは、移動しなくても、あらかじめ定められたサンプリングタイムが経過すると(ステップS2にて肯定判断)、再度周囲画像を取り込むと共に、自己の姿勢情報を検出する。そして、当該画像においては、上記移動前工程にて抽出した特徴点と同一の特徴点を抽出し、その特徴点までの現在の相対位置を測定する。また、同時に、現在の自己の姿勢情報をも検出する。これらの情報(特徴点の相対位置情報、姿勢情報)は、メイン処理部10に送信され、メインコンピュータ45に記憶される(ステップS3、移動後工程)。
【0051】
続いて、時間的に前後するそれぞれの時点における特徴点の相対位置情報と、移動体1自身の姿勢情報とがメインコンピュータ45に送信されると、当該コンピュータ45では、まず、姿勢情報から移動体自身の姿勢変化量である姿勢変化情報が算出される(ステップS4)。その後、この姿勢変化情報と、それぞれの時点における特徴点までの各相対位置情報とをあらかじめ定められた式(後述する数9式)に代入することで、移動体1自体の時間的に前後する先の時刻の位置に対する後の時刻の自己位置が算出される(ステップS5、演算工程)。
【0052】
そして、上記処理を繰り返すことにより、それぞれの時点における自己位置を算出することができるため、移動体1の移動経路を算出することができる。具体的には、さらに次のサンプリングタイムが経過すると、その経過時における取得画像などが新たな後の時刻におけるものとなり、その際に上記において後の時刻のものが新たな先の時刻のものと更新されるため、かかる2点間における相対的な移動量を算出することができる。
【0053】
ここで、上記ステップS5の演算工程にて用いる式について説明する。なお、ステレオカメラ2は他の実施形態において説明するように固定されていなくてもよいが、本実施形態においては、上述したように、移動体1にステレオカメラ2が固定されている場合について説明する。そして、移動前(先)の時刻をt、移動後(後)の時刻をt+1として、移動前に図1(a)に位置する移動体1が、図1(b)に位置するよう移動した場合について説明する。
【0054】
図1において、
XB0:時刻tのときのボディ座標系
XC0:時刻tのときのカメラ座標系
XB1:時刻t+1のときのボディ座標系
XC1:時刻t+1のときのカメラ座標系
B0XP:時刻tのときの点Pのボディ座標
C0XP:時刻tのときの点Pのカメラ座標
B1XP:時刻t+1のときの点Pのボディ座標
C1XP:時刻t+1のときの点Pのカメラ座標
φ:移動体1の移動前後における姿勢角(ロール角)の変化量
θ:移動体1の移動前後における姿勢角(ピッチ角)の変化量
ψ:移動体1の移動前後における姿勢角(ヨー角)の変化量
である。
【0055】
今、XC0からXB0への同時変換行列をB0 C0Sとすると、移動体1に対してカメラが固定であれば、移動の前後でB0 C0Sは不変(つまりB1 C1S=B0 C0S)であるから、以下の式のようになる。
【0056】
【数1】
【0057】
【数2】
【0058】
ちなみに、上記同時変換行列B0 C0Sは、ステレオカメラ2が固定されていることにより、あらかじめ定められた行列である。
【0059】
また、XB0とXB1の間の同時変換行列をB0 B1S、並進変換行列をB0 B1T、回転変換行列をB0 B1Rとし、XB0とXB1の間の並進移動量をtx,ty,tzとすれば、
【0060】
【数3】
【0061】
【数4】
【0062】
【数5】
【0063】
【数6】
となる。ここで、
【0064】
【数7】
とすれば、
【0065】
【数8】
となるので、
【0066】
【数9】
と表せる。
【0067】
従って、あらかじめ定められたB0 C0Sを含む数9式に、測定した点Pの座標であるC0XP及びC1XP,さらに、姿勢角の変化量にて表されるB0 B1Rを代入することで、移動体1の時刻tに対する時刻t+1のときの移動量(tx,ty,tz)を求めることができる。
【0068】
続いて、図3のステップS1及びステップS3に示す特徴点の位置測定の手法について、図4乃至図6を参照して詳述する。図4は、特徴点の位置測定の動作を示すフローチャートである。図5乃至図6は、ステレオカメラ2にて取得した画像の処理の様子を示す図である。
【0069】
まず、特徴点の位置測定の概略を説明する。基本的には、特徴点を選定し、当該特徴点の位置を測定する(ステップS13,14,15)、という手順となるが、移動後における特徴点の位置測定では移動前に選定した特徴点を追跡(トラッキング)してその位置を測定する(ステップS16,14,15)、という手順となる。はじめに、移動体1が移動前(先)の位置にいる場合の特徴点の抽出、及び、その位置測定の動作について説明する。
【0070】
まず、移動体1は、メインコンピュータ45、あるいは、画像処理部41の指示により、ステレオカメラ2にて周囲の画像を取得する(ステップS11、図5(a))。このとき、一方のカメラから取得したマスター画像と、他方のカメラから取得したスレーブ画像とを、図5(a)に示すように、一つのフレームメモリ上の奇数フィールド、偶数フィールドにそれぞれ格納する。なお、これにより、NTSC用の画像処理装置において、構成のコンパクト化を図ることができる。
【0071】
ここで、本実施形態では、ステレオ視した画像から圧縮によりマスター画像を消去してスレーブ画像を取得し(図5(b))、このスレーブ画像を画像処理部41に一時的に記憶する。これと共に、画像の偶数ライン、奇数ラインを調整し(図5(c))、圧縮によりスレーブ画像を消去し、マスター画像を取得する(図5(d))。そして、このマスター画像を、スレーブ画像同様に画像処理部41に一時的に記憶すると共に、当該マスター画像のコピーを(図5(e))、移動前(先の時刻)におけるマスター画像として、画像処理部41あるいはメインコンピュータ45に別途記憶しておく。これは、移動後にトラッキングするはずの特徴点を見失った時などに用いる画像である。
【0072】
ここで、取得したスレーブ画像(図5(b))とマスター画像(図5(d))とを、メイン処理部10に送信してモニタ10bに表示する。これにより、遠隔地にいるオペレータは、移動体1から見た周辺状況を視認することができる。
【0073】
ちなみに、画像処理部41に記憶されたマスター画像などの画像は、メインコンピュータ45に記憶されてもよい。これは、特徴点の抽出処理を画像処理部41で行うか、あるいは、メインコンピュータ45で行うかの違いによるが、いずれの演算手段で行っても構成に相違はない。従って、この時点では、マスター画像(d)、スレーブ画像(b)、マスター画像のコピー(e)、輪郭抽出画像(g)が画像処理部41あるいはメインコンピュータ45に記憶されていることとなる。
【0074】
続いて、今は移動前における特徴点の抽出処理であり、いまだ特徴点(トラッキングポイント)を選定していない状況にあるので、新たな特徴点の選定である抽出処理を行う(特徴点の更新要求あり(ステップS12で肯定判断))。このため、本実施形態では、一時的に保持しているマスター画像(図5(d))を二値化処理し(図5(f))、輪郭抽出処理を行う(図5(g))。この画像に基づいて、あらかじめ定められた判断基準に適合する箇所を特徴点として選定すべく、図6に示す処理を行う。但し、特徴点を選定する手順は、以下に示すものに限定されない。
【0075】
まず、図6(a)に示すように、輪郭抽出画像を9つの領域に分割する。なお、分割領域に画面の淵を含ませないようにしたのは、移動後に特徴点が当該画像から外れてしまうのを防ぐためである。そして、ある領域において矩形テンプレートウインドウW1(例えば、20×20ピクセル)を設定し、当該テンプレートウインドウを、符号W2,W3に示すように左上から右下まで数ピクセルずつ移動させて、当該テンプレート内の濃淡特徴量を抽出する(図6(b))。このとき、対象となる画像は2値の輪郭画像であるから、テンプレート内に白い輪郭線がある場合のみ特徴量を検出し、そのときのテンプレート左上の点の座標を保存しておく(例えば、テンプレートウインドウW2についての点C)。なお、処理対象となる領域は、9つのうち少なくとも一つでよく、図6(b)に示すように左上の領域のみを処理するようあらかじめ定めておいてもよい。但し、処理領域については、他の実施形態において説明するように、移動体1の動作状況に応じた箇所の複数の領域を処理対象としてもよい。
【0076】
このようにして、特徴量の検出できた全ての点の座標を保存したら、二値画像ではないマスター画像において保存した各座標におけるテンプレート内の濃淡特徴量を抽出する(図6(c),(d))。本実施形態では、濃淡特徴量を、テンプレートW2内で最大濃度の画素と最小濃度の画素との濃淡差、とする。そして、特徴量(濃淡差)が最大となるテンプレートを特徴点とし、その点の座標を保存しておく(特徴点の検出、ステップS13)。但し、この特徴点を算出する手法は一例であって、この手法に限定されるものではない。
【0077】
続いて、マスター画像において抽出した特徴点に対応する点(マッチングポイント)を、スレーブ画像上で検索する(ステップS14)。このため、まず、マスター濃淡画像(図5(d))において特徴点(特徴点に対応するテンプレートウインドウW2)の濃淡特徴量を抽出し、スレーブ濃淡画像(図5(b))上で正規化相関をとる。これによりスレーブ画像上において特徴点に対応する点が見つかったらその点の座標を保存しておく。なお、正規化相関によるマッチングについては、従来より用いられている周知技術であるので、その詳細な説明は省略する。
【0078】
その後、マスター画像上で検出した特徴点(トラッキングポイント)と、スレーブ画像上で検出した特徴点に対応する点(マッチングポイント)との座標から、画像処理部41あるいはメインコンピュータ45などの演算装置にて、ステレオ視の原理より特徴点の三次元座標が計測される(ステップS15)。計測された三次元座標は、移動前すなわち図1(a)における時刻tのときの点Pのカメラ座標C0XPとして、メインコンピュータ45に記憶される。ここで、ステレオ視による三次元座標の計測については、周知技術であるのでその説明は省略する。
【0079】
ちなみに、移動前における特徴点の抽出処理でなくとも、前回の画像において抽出した特徴点のトラッキングに失敗したり、何らかのエラーが出たために特徴点の更新要求がある場合には、別途記憶しておいた先の位置におけるマスター画像(図5(e))を用いて、特徴点の抽出処理が行われる(ステップS12で肯定判断、ステップS13)。
【0080】
そして、移動体1が移動した後、すなわち、前回の画像取得から所定時間が経過して時刻がt+1となった時(図1(b))には、再度ステレオカメラ2にて画像を取得し(ステップS11)、特徴点の追跡を行う。かかる場合には、特徴点の更新要求がないため(ステップS12にて否定判断)、新たに取得した画像中から先の画像にて抽出した特徴点と同一の特徴点のマッチングによる検出を行う(ステップS16,S14)。具体的には、先のマスター濃淡画像中において抽出した特徴点の濃淡特徴量を抽出し、現在のマスター濃淡画像上で正規化相関をとる。そして、対応する点が検出できたらその点の座標を保存しておく(ステップS16)。また、当該現在のマスター画像中の特徴点に対応する点を、上述したようにスレーブ画像上で正規化相関をとり、対応する点が検出できたらその点の座標を保存しておく(ステップS14)。
【0081】
このようにして、移動後(時刻t=t+1)におけるマスター、スレーブの各画像上の特徴点の座標から、移動体1から見た点Pのカメラ座標C1XPを、ステレオ視の原理より計測する(ステップS15)。
【0082】
ちなみに、上述したように、画像取得時(ステップS11)には、移動前(時刻t=t)、移動後(t=t+1)のいずれのときにおいても、移動体1の姿勢角の検出が行われる。かかる情報は、移動体自体のCPUに保持されていてもよく、メインコンピュータ45に記憶されてもよい。そして、移動後の姿勢角情報が検出された後には、移動前と比較してその変化量(φ,ψ,θ)が算出される。これら変化量の各値は、上述した数5式に示すように、回転変換行列をB0 B1Rとして表される。
【0083】
このようにして測定されたC0XP,C1XP,B0 B1Rから、あらかじめ定められているB0 C0Sを含む数9式を用いて移動体1の並進移動量がメインコンピュータ45などの演算手段にて算出される。そして、引き続き、移動後(t=t+1)の位置を新たな移動前の位置とし、その後の移動後の位置を新たな移動後(t=t+2)とすることで、この間の移動体1の移動量を測定することができる。これらを繋ぎ合わせることで、移動体1の移動経路及び現在位置を計測することができる。
【0084】
従って、移動体1の姿勢角と、当該移動体1に搭載されたカメラ2にて、少なくとも一つの特徴点を追跡することで自己位置を計測することができるため、安定して自己位置の計測を行うことができると共に、その計測精度の向上を図ることができる。
【0085】
〈第2の実施形態〉
次に、本発明の第2の実施形態について、図7を参照して説明する。図7は、本発明の第2の実施形態における動作の様子を示す概略図である。特に、図7(a)は、時刻tの時の様子を示す図であり、図7(b)は、時刻t+1の時の様子を示す図である。
【0086】
なお、本実施形態における以下の動作も、上述した第1の実施形態と同様に、メインコンピュータ45や移動体1内の画像処理部41やCPU42,43,44内に、あらかじめ各動作用プログラムが組み込まれることで実現される。
【0087】
本実施形態においては、上述した第1の実施形態における移動体1すなわち自己位置計測装置とほぼ同一の構成を有しており、また、ほぼ同一の方法にて自己位置を計測する。但し、本実施形態では、ステレオカメラ20が移動体1に固定されていない点で、上記第1の実施形態とは異なる。
【0088】
図7(b)に示すように、ステレオカメラ20を移動体1に固定せず、例えば、図2に示すメイン処理部10からオペレータが遠隔操作することにより可動可能である。すると、当該ステレオカメラ20のパン・チルト角や、高さなどを変えることにより、より有益な視覚情報(画像)を得ることができる場合がある(符号20’は、ステレオカメラが可動した時の様子を示す)。そして、このような場合にも、以下の算出式を用いることで、移動体1の並進移動量を求めることができる。ここで、ステレオカメラの固定位置に対する可動量、すなわち、図7(b)の符号20に示す位置から符号20’の位置に可動したときの三次元の並進移動量(a,b,c)及び回転移動量(ロール角α、ピッチ角β、ヨー角γ)は、ポテンショメータやロータリーエンコーダなどのセンサを当該カメラに備えることで、検出することができる。
【0089】
図7において、
XC1:カメラが固定である場合の時刻t+1におけるカメラ座標系
XC1’:カメラが固定でない場合の時刻t+1におけるカメラ座標系
C1XP:カメラが固定である場合の時刻t+1時の点Pのカメラ座標
C1’XP:カメラが固定でない場合の時刻t+1時の点Pのカメラ座標
である。そして、移動の前後でのステレオカメラの移動体ボディ座標に対するカメラ座標の並進移動量がa,b,cであり、回転移動量がα,β,γであるとすると、XC1’からXC1への同時変換行列C1 C1’Sは、
【0090】
【数10】
【0091】
【数11】
【0092】
【数12】
【0093】
上記のようになるので、オペレータの操作によるステレオカメラの移動体ボディ座標に対するカメラ座標の並進移動量a,b,c及び回転移動量α,β,γの値を用いることで、その同時変換行列C1 C1’Sを算出することができる。従って、移動体1の並進移動量は、ステレオカメラ20が固定のときと同様に、
【0094】
【数13】
【0095】
となる。従って、まず、カメラが可動したときの並進移動量(a,b,c)、回転移動量(α,β,γ)を用いてC1 C1’Sを算出し、B0 C0Sと共に数13式に代入しておき、当該式をあらかじめ定められた式として用いる。そして、この数13式に第1の実施形態と同様に、移動前後における移動体1から特徴点である点Pまでの位置座標と、移動体の姿勢角変化量を代入することで、当該移動体1の自己位置を計測することができる。
【0096】
〈第3の実施形態〉
以下、本発明の第3の実施形態について、図8を参照して説明する。本実施形態においては、上述した他の実施形態とほぼ同一の構成を有しているが、自己位置を計測する方法が、以下の点で異なる。なお、以下の動作は、画像処理部41やメインコンピュータ45などの演算手段に以下の方法を実現すべく演算を行うプログラムが組み込まれることで実現される。
【0097】
まず、移動体1は、移動前工程にて、あらかじめ定められた判断基準に基づいて複数の特徴点を選定すると共に、それぞれの特徴点の相対位置を測定する。例えば、上述したように、取得画像を9つの領域に分割して、その中から4つの領域を選択し、それぞれの領域から特徴点を抽出する。4つの領域の選択は、移動体1の動作モードに応じて行うが、その一例を図8(a)〜(d)に示す。これらの図において、それぞれ斜線領域が特徴点を抽出する領域であるが、図8(a)は、移動体が前進する場合、図8(b)は後退する場合、図8(c),(d)は、それぞれ移動体が右回転、左回転する場合である。
【0098】
そして、上述したように、マスター画像において特徴点の抽出を行ったら、スレーブ画像において特徴点のマッチングを行い、当該特徴点の座標を計測する。そして、これは4点についてそれぞれ行う。その後、移動体の移動後には、上記抽出した4つの特徴点をトラッキングし、それぞれの座標を計測する。このときもその計測手法は、上述した手法と同様である。
【0099】
ここで、例えば、移動体1が前進する場合には、図8(a)の斜線4つの領域を特徴点の抽出領域として選択したが、移動体の移動方向が矢印Y1方向であると特徴点の移動方向が矢印Y2となるため、特徴点が画像内に収まり、トラッキングを継続することができる。そして、それぞれの場合も同様に、移動体が移動しても特徴点が領域内に収まるような処理領域となっている。
【0100】
そして、それぞれの特徴点毎に、上述した手法と同様の手法にて、移動体1の移動前に対する移動後の自己位置を算出する。このとき、複数の特徴点にうち、トラッキングができない点も生じてくる。かかる場合には、より長い期間トラッキングを継続した点を信頼し、移動量を算出する。ちなみに、トラッキングにて特徴点を見失った場合には、新たな特徴点の更新、抽出要求が出され、抽出された新たな特徴点について位置計測が行われる。
【0101】
このようにすることにより、複数の特徴点をトラッキングすることで計測値の高精度化を図ることができると共に、取得画像に対する処理対象領域を必要の範囲内にすることができ、装置の処理負担を抑制することができる。また、移動体の動作モードに応じて処理領域を設定することで、特徴点のトラッキングを長時間継続することができ、移動体の位置計測精度のさらなる向上を図ることができる。
【0102】
ここで、上述した特徴点の抽出は、4点に限定されるものではない。上述した第1の実施形態のように1点でもよく、他の数の複数点でもよい。また、4点のうち信頼できる特徴点における計測データを採用することに限定されず、複数の特徴点についてその平均値を採ってもよく、また、最小自乗法などを用いて計測値を算出してもよい。
【0103】
〈第4の実施形態〉
次に、本発明の第4の実施形態について説明する。本実施形態では、複数の特徴点を抽出して移動体自体の自己位置を算出するものであるため、上述した第3の実施形態とほぼ同様の構成であるが、さらに、以下の構成を採っている。なお、以下の説明においては、図1乃至図8を必要に応じて用いることとする。
【0104】
また、以下に示す動作は、上述した他の実施形態と同様に、メインコンピュータ45や移動体1内のCPUなどに、あらかじめ各動作用プログラムが組み込まれることで実現できる。すなわち、メインコンピュータ45や移動体1内の画像処理部、CPUなどに、以下の動作を実現する機能が構築されており、当該メインコンピュータ45等に位置測定に関する各データが送信され、以下の処理が行われる。
【0105】
本実施形態では、まず、上述した他の実施形態と同様に、位置測定手段2はステレオ視可能な複数のカメラによるステレオカメラにて構成されており、図3のステップS1及びステップS3に示す上記移動前工程及び移動後工程では、ステレオカメラを用いて,各カメラによる複数の取得画像をマッチングすることにより各画像毎にそれぞれ対応する各特徴点を検出し,ステレオ視の原理により当該各特徴点の相対位置を測定する。その具体的な相対位置の測定手法は、上述したので省略する。
【0106】
そして、図3のステップS3に示す移動後工程では、各カメラにて取得した画像をマッチングする際に、当該マッチングによる一致度に関する値を算出して各特徴点毎に対応付けて演算手段に記憶する。具体的には、移動体1が所定のサンプリングタイム経過後(移動後)に、画像を取得して特徴点の追跡(トラッキング)を行うが、まず、一方のカメラにて取得したマスター画像から、上述した濃淡差を濃淡特徴量として移動前工程において選定した特徴点と同一の特徴点を検出する(図4のステップS16)。その後、当該マスター画像上の特徴点に対応する点を、スレーブ画像で正規化相関をとり、マッチング処理を行うことにより検出する(図4のステップS14)。このとき、正規化相関値は、マッチングした画像の一致度を表しており、0〜1の間の値をとるよう設定されている。例えば、完全に画像パターンが一致する場合に1となるようにする。本実施形態では、かかる値を「重み」として用い、複数の特徴点から求められた各自己位置である移動量のうち、どの移動量を重視して採用するか、ということの割合を示す値となる。
【0107】
従って、演算工程では、複数の特徴点毎に計測した自己位置の各計測値に,各特徴点の一致度に関する値に応じた重みを付けて最終的な自己位置を算出する。そして、このとき、特徴点を検出する際のマスター画像とスレーブ画像とのマッチングによる一致度が高くなるにつれて各計測値に付ける重みが大きくなるよう設定される。
【0108】
具体的には、n個の特徴点がトラッキングできている場合に、上述したように、数9式あるいは数13式を用いて各特徴点の相対位置から算出した移動体1の移動量x1〜n(ここでは、x方向の移動量のみを考える)に、上記相関値c1〜nを重みとして付加し、これらの加重平均mwを数14式に示すように算出する。
【0109】
【数14】
【0110】
そして、各特徴点毎に、各移動量(x,y,z)それぞれの加重平均mwを求め、かかる値を最終的な移動量の計測値とする。
【0111】
これにより、ステレオカメラによるマッチング時の一致度が高い特徴点、すなわち、相関値が大きい特徴点から算出された移動量の割合が大きく反映されて、移動体1の最終的な移動量が算出されることとなる。そして、相関値が大きい特徴点は、静止物体上の特徴点である可能性が高いため、他の移動物体をトラッキングして移動量が算出されることが抑制されるため、より精度よく、移動体の現在位置を計測することができる。
【0112】
〈第5の実施形態〉
次に、本発明の第5の実施形態について説明する。本実施形態では、上記第4の実施形態と同様に、複数の特徴点を抽出して各特徴点毎に装置自体の移動量を算出し、特徴点毎の移動量に重みを付けて最終的な自己位置を算出する、という構成である。但し、本実施形態では、上記数14式に示す加重平均を求める際の重みの設定方法が異なり、以下の構成を採っている。なお、以下の説明においては、図1乃至図8を必要に応じて用いることとする。
【0113】
また、以下に示す動作は、上述した他の実施形態と同様に、メインコンピュータ45や移動体1内の画像処理部、CPUなどに、あらかじめ各動作用プログラムが組み込まれることで実現できる。すなわち、メインコンピュータ45や移動体1内のCPUなどに、以下の動作を実現する機能が構築されている。
【0114】
本実施形態においては、図3のステップS1及びステップS3に示す上記移動前工程及び移動後工程では、画像取得手段であるステレオカメラにて構成された位置測定手段2を用いて,取得画像の濃淡値に基づいて複数の特徴点を選定及び検出し,当該各特徴点の相対位置を測定する。
【0115】
このとき、移動前工程は、各特徴点を選定する際に,当該各特徴点の濃淡値に関する情報、具体的には、マスター画像上における所定領域の最大濃淡値と最小濃淡値との差を濃淡特徴量として、最も高い領域上の点を特徴点として選定する(図4のステップS14)が、本実施形態では、特に、選定した複数の特徴点毎に対応付けて、算出した濃淡特徴量をメインコンピュータ45や移動体1内のCPU、あるいは、移動体1内に設けられた所定の記憶部などに記憶する。
【0116】
また、演算工程では、複数の特徴点毎に計測した自己位置の各測定値に,各特徴点の濃淡特徴量に応じた重みを付けて最終的な自己位置を算出する。このとき、重みは0〜1の値であるが、濃淡特徴量が大きければ大きいほど、1に近づく値として設定される。具体的には、数15式に示す式にて算出する。なお、wが重みとなり、数14式におけるcと同様に働く。すなわち、wは、それぞれ特徴点の数nだけ算出され、w1〜nまで算出される。また、gは濃淡差であり、これも各特徴点毎に設定される。
【0117】
【数15】
【0118】
なお、重みを上式のように算出するのは、画像の濃度は通常8ビットデータとして扱われ、濃淡差は0〜255の値をとる。従って、これを0〜1の値に変換するためである。
【0119】
このようにして求めた重みw(w1〜n)は、特徴点毎に求められ、上記数14式と同様に加重平均が算出される。これにより、画像の濃度が急激に変化する部分(エッジ)を特徴点として選定していることとなり、かかる特徴点の信頼度は高いと考えられる。また、このような特徴点は静止物体上の特徴点であるとも考えられる。従って、濃淡特徴量が大きいものの移動量の割合を大きく反映して最終的な移動量を算出することで、他の移動物体の影響を抑制して、より精度よく移動体1の現在位置を計測することができる。
【0120】
〈第6の実施形態〉
次に、本発明の第6の実施形態について説明する。本実施形態では、上記第4乃至5の実施形態と同様に、複数の特徴点を抽出して各特徴点毎に装置自体の移動量を算出し、特徴点毎の移動量に重みを付けて最終的な自己位置を算出する、という構成である。但し、本実施形態では、数14式において移動体1の最終的な移動量である加重平均を求める際の重みの設定手法が異なり、以下の構成を採っている。なお、以下の説明においては、図1乃至図8を必要に応じて用いることとする。
【0121】
また、以下に示す動作は、上述した他の実施形態と同様に、メインコンピュータ45や移動体1内のCPUなどに、あらかじめ各動作用プログラムが組み込まれることで実現できる。すなわち、メインコンピュータ45や移動体1内のCPUなどに、以下の動作を実現する機能が構築されている。
【0122】
本実施形態における図3のステップS1及びステップS3に示す移動後工程及び演算工程は、演算工程によって自己位置の算出された後に繰り返し作動すると共に,その際に,直前に作動した移動後工程にて抽出された複数の特徴点を移動前工程にて選定した複数の特徴点として作動する。すなわち、第1の実施形態においても説明したように、移動体1が移動中に、連続して特徴点の追従を行い、その移動量を積算していくことで経路を算出する構成となっている。
【0123】
そして、特に、移動後工程では、各特徴点を抽出する際に,当該各特徴点が連続して検出された回数の値を、特徴点に対応させてメインコンピュータ45や移動体1内の画像処理部など、あるいは、所定の記憶部に記憶する。具体的には、すべての特徴点毎に連続してトラッキングした回数を積算し、特徴点毎に記憶する。このとき、例えば、特徴点の識別番号及び当該特徴点の濃淡特徴量と共に記憶することで、他の特徴点とのトラッキング回数を区別して記憶することができる。そして、新たに選定された特徴点は、そのトラッキング回数が初期化されて、1回からカウントされ、トラックキング回数が増える毎に記憶されている値が更新される。また、これらトラッキング回数のうち、最も大きい値は、最高回数はlmaxとして別途メインコンピュータ45や画像処理部、さらには、移動体1内の別の記憶部などに記憶される。この最高回数の値は、いずれかの特徴点のトラッキング回数が更新される毎に当該各特徴点のトラッキング回数と比較され、かかる値が最高回数の値よりも大きい場合には更新される。
【0124】
このようにして計測されたトラッキング回数は、最終的な移動量を算出する際に利用される。すなわち、演算工程では、自己位置の測定を複数回行った後に,複数の特徴点毎に計測した自己位置の各測定値に,各特徴点の検出回数値に応じた重みを付けて最終的な自己位置を算出する。このとき、重みwは、トラッキング回数をl、その最大値をlmaxとすると、数16式にように表され、すなわち、検出回数値が多くなるにつれて各計測値に付ける重みが大きくなるよう設定される。これにより、重みは0〜1の間に設定され、最大回数をカウントしている特徴点については、lmax=lとなり、その重みが1となる。なお、wは、数14式におけるcと同様に働く。すなわち、wは、それぞれ特徴点の数nだけ算出され、w1〜nまで算出される。また、lの値も各特徴点毎に設定される。
【0125】
【数16】
【0126】
その後は、上述した第5の実施形態と同様に、求めた重みwを用いて上記数14式のように加重平均が算出される。これにより、視界から外れていないにも関わらず、少ない回数しか連続してトラッキングできないような不安定な特徴点から求められた移動量の影響を抑制することができ、一方で、連続して多くトラッキングできた特徴点からの移動量の割合が大きく反映されるため、信頼性の高く精度のよい移動体の現在位置を計測することができる。なお、上述したような不安定な特徴点は、他の移動物体上の特徴点であるとも考えられるため、そのような移動物体の影響を抑制することができる。
【0127】
〈第7の実施形態〉
次に、本発明の第7の実施形態について説明する。本実施形態では、上記第4乃至6の実施形態と同様に、複数の特徴点を抽出して各特徴点毎に装置自体の移動量を算出し、特徴点毎の移動量に重みを付けて最終的な自己位置を算出する、という構成である。但し、本実施形態では、数14式において移動体1の最終的な移動量である加重平均を求める際の重みの設定手法が異なり、以下の構成を採っている。なお、以下の説明においては、図1乃至図8を必要に応じて用いることとする。
【0128】
また、以下に示す動作は、上述した他の実施形態と同様に、メインコンピュータ45や移動体1内の画像処理部、CPUなどに、あらかじめ各動作用プログラムが組み込まれることで実現できる。すなわち、メインコンピュータ45や移動体1内の画像処理部やCPUなどに、以下の動作を実現する機能が構築されている。
【0129】
まず、本実施形態においては、上記第6の実施形態と同様に、図3のステップS1及びステップS3に示す移動後工程及び演算工程は、演算工程によって自己位置の算出された後に繰り返し作動すると共に,その際に,直前に作動した移動後工程にて抽出された複数の特徴点を移動前工程にて選定した複数の特徴点として作動する。すなわち、第1の実施形態においても説明したように、移動体1が移動中に、連続して特徴点の追従を行い、その移動量を積算していくことで経路を算出する構成となっている。
【0130】
そして、複数の特徴点から求められた移動体の移動量には、各特徴点毎に重みが付けられて、数14式に示すよう加重平均を求めて最終的な移動量が算出されるが、本実施形態では、これまでの移動経路から予測された次の自己位置からのずれ量に応じた重みが用いられる。以下に詳述する。
【0131】
まず、演算工程は、これまでに複数の特徴点からの自己位置に基づいて総合的に算出した最終的な自己位置に関する値を、メインコンピュータ45などに記憶しておき、後に移動体1の経路が参照できる。
【0132】
そして、演算工程は、自己位置の測定を複数回行った後に、以下の工程に示す動作を行う。すなわち、これまでの自己位置に関する値に基づいて装置自体の自己位置を予測して予測自己位置を算出する予測値算出工程と、複数の特徴点毎に計測した自己位置の各測定値と予測自己位置とを比較してそのずれ量をそれぞれ算出する比較工程と、複数の特徴点毎に計測した自己位置の各測定値に,各特徴点毎のずれ量に応じた重みを付けて最終的な自己位置を算出する最終自己位置算出工程と、である。なお、以下に説明するように、上記ずれ量が小さくにつれて各計測値に付ける重みが大きくなるよう設定される。
【0133】
上記予測値算出工程は、一定のサンプリング間隔で得られた自己移動量の時系列データから、次の瞬間の移動量を予測する。これは、移動体であるロボットの運動が一定の物理法則(運動方程式)に基づいており、極端に飛んだ値が計測されることはないという理由からである。具体的には、カルマンフィルタを用いて予測移動量を算出する。
【0134】
ここで、カルマンフィルタとは、あるシステムにおいて変化量の時系列データが測定できるときに、その変化の仕方をモデル化し、その変化量の測定値とシステムがどれだけの外乱を受けるか、というデータをあらかじめ与えておくことで、その変化量の予測・濾波・平滑を精度よく行うための手法である。但し、カルマンフィルタの詳細については省略し、以下、予測値の算出手法を説明する。
【0135】
カルマンフィルタを適用すべく、まず、システムを次式の基本システムで表すこととする。
【0136】
【数17】
【0137】
【数18】
【0138】
今、求めたい予測値は移動体の移動量であるので、これを(x,y,z)として、その速度、加速度を、それぞれ(vx,vy,vz)、(ax,ay,az)とすると、数19式のようになる。
【0139】
【数19】
【0140】
そして、y,zについても同様である。このとき、以下のように定義する。
【0141】
【数20】
【0142】
【数21】
【0143】
【数22】
【0144】
【数23】
【0145】
そして、上式のようにすると、カルマンフィルタのアルゴリズムから、以下の式を得られる。
【0146】
【数24】
【0147】
となる。Kkはカルマンゲイン、Σx0は、x0の共分散行列である。ここで、数24式の最下段にある式の右辺は、一般に未知であるため、当該式は決定できない。そこで、数24式の最上段式、及び、上から4番目の式の初期値として、以下の数25式を採用する。
【0148】
【数25】
但し、O6は、6次元の零ベクトル、I6×6は、6×6の単位行列を表す。
【0149】
【数26】
【0150】
また、上式のように定義したのは、観測雑音の分散の不定性を、上式の右式の初期値の不定性の中にまとめて取り込むためである。ここで、雑音の分散比σw 2/σv 2が最終的に未知パラメータとしてカルマンフィルタに残る。
【0151】
以上より、システム雑音及び観測雑音の分散比σw 2,σv 2を決定すれば、移動量の予測値を含む状態予測ベクトルとして数24式の上から2番目の式が、逐次得られる。なお、以下に説明する実施例では、上記分散比をトライアンドエラーにより決定する。
【0152】
以上のように、カルマンフィルタによって算出される移動量の予測値は、過去の移動量の時系列データをもとに算出されるため、計測開始時点で視界内に移動物体がなければ、算出される予測値にも移動物体の影響は入らない。そして、他の移動物体上の特徴点から算出した移動量は予測値から大きなずれが生じると考えられるため、複数得られた特徴点毎の移動量に対して予測値からのずれ量が大きいほど小さな荷重をかけ、一方、ずれ量が小さいほど大きな加重がかかるようなフィルタをかければ、他の移動物体の影響を抑制することができる。これにより、より制度の高い移動量を算出することができる。
【0153】
ここで、本実施形態では、統計の理論を応用し、予測値からのずれは、予測値を平均とする正規分布に従うと考え、実測値をもとに正規分布の分散σ2を決定し、この正規分布関数の値を重みとして使用する。すなわち、各特徴点から計測された移動量が予測値と同一の場合に重みを1とし、当該予測値から離れるにつれて0に近づくよう、重みを設定する。なお、正規分布関数は、以下に説明するように、トライアンドエラーにより適切に設定し、かかる分布をメインコンピュータ45や移動体1内のCPUや所定の記憶部に記憶保持しておく。
【0154】
(実施例)
ここで、上述したように、カルマンフィルタを用いて移動体1の移動量の予測値を算出し、当該予測値に基づいた重みを使用して、複数の特徴点から自己位置を計測した実験を説明する。この実験では、図1乃至図2に示す自己位置計測システムが搭載された移動体1を前進走行させて、その移動体1のカメラ視界内で他の移動物体100を移動させ、かかる環境下で移動体1の移動量を計測する。そして、上記カルマンフィルタの有効性を検証するというものである。
【0155】
ここで、かかる実験では、移動体1の本システムにより計測した移動量と比較する実際の移動量の計測(デッドレコニング)も必要になる。これについては、車輪の回転数とステア角をロータリエンコーダで測定して、これを積分して位置計測を行う手法であるオドメトリによる位置推定を行い、かかる計測値を理論値として取り扱う。なお、実験環境では、移動体1が前進走行する床は平面であり、滑りも起こりにくいため、直進移動距離の計測は誤差率1%以内である。
【0156】
また、実験で使用する他の移動物体100は、自己位置を計測する移動体1とは異なる動きをするものであり、その構成の概要を図9に示す。この図に示すように、他の移動物体100は、シャシ101に図示しない駆動源により駆動される自走用の車輪102を備え、また、移動体1が認識しやすい特徴点となりうる黒いフロックボール103を4つ高い位置に備えている。
【0157】
さらに、上述した数24式の上から第4式に示すシステム雑音及び観測雑音の分散比σw 2,σv 2、及び、測定値と予測値とのずれの分散σ2を決定する必要がある。ここで、σv 2については、あらかじめ測定でき、一方、σw 2についてはトライアンドエラーにより決定した。また、σ2は、σw 2,σv 2を決定した後に測定した値をもとにトライアンドエラーを行って調節した。そして、これらの値は、各数式に代入されたり、あるいは、自己位置を計測する際に参照できるよう、メインコンピュータ45や移動体1内のCPUや記憶部などに記憶されている。
【0158】
このようにして、実際に実験を行ったときの様子を、図10乃至図13に示す。ここで、図10(a)に示す例は、自己位置計測装置自体である移動体1を、壁200に向かって静止させた状態で、他の移動物体100を矢印A100に示すように、ラインL100に沿って移動させる場合である。このとき、移動体1の前方には、種々の特徴点となりうる物体(図示せず)が存在しており、また、壁200には、特徴点となる図形が描かれている。
【0159】
そして、図10(b)に、移動体1の自己位置の計測結果を示す。横軸には計測時間t[s(秒)]をとり、縦軸には移動体1の壁方向200への移動距離tx[mm]をとる。この図に示すように、静止している移動体1において、その計測値はほぼ0の値を示している。すなわち、静止時には他の移動物体100の影響をほぼ受けずに精度の高い自己位置の計測が可能である。なお、かかる実験においては、移動量として壁200方向への距離xのみを計測しているが、これに限定されない。もちろん、上述してきたように、三次元(x,y,z)における移動量も測定可能である。
【0160】
次に、図11(a)に示すように、他の移動物体100を矢印A100方向に、ラインL100に沿って移動させ、また、移動体1を矢印A1方向に前進させた場合を説明する。具体的には、自己位置を計測する移動体1が、他の移動物体100に追い越される環境において計測を行った。そして、そのときの自己位置の計測結果を図11(b)に示す。この図では、図10(b)と同様に、横軸には計測時間t[s(秒)]をとり、縦軸には移動体1の移動距離tx[mm]をとっており、デッドレコニングによる移動量の理論値(実線)と、本システムによる移動量の計測値(菱形プロット)を示している。すると、当該計測値はほぼ理論値と差がなく、高精度な自己位置の計測が可能であることがわかる。
【0161】
さらに、図12(a)に示すように、他の移動物体100を矢印A101方向に、ラインL101に沿って移動させ、また、移動体1を矢印A1方向に前進させた場合を説明する。具体的には、自己位置を計測する移動体1が、他の移動物体100とすれ違う環境において計測を行った。そして、そのときの自己位置の計測結果を図12(b)に示す。そして、この図には、デッドレコニングによる移動量の理論(実線)値と、本システムによる移動量の計測値(菱形プロット)とを示したが、このような実験例においても、当該計測値はほぼ理論値と差がなく、高精度な自己位置の計測が可能であることがわかる。
【0162】
以上のように、予測値に基づくフィルタを用いて自己位置の計測を行うことで、カメラの視界内に他の移動物体100が存在している場合であっても、当該他の移動物体100の存在の影響を抑制して、高精度な自己位置計測を実現できる。すなわち、他の移動物体100を特徴点として追従した場合であっても、その影響を軽減して、自己位置を計測することができる。
【0163】
〈第8の実施形態〉
次に、本発明の第8の実施形態について説明する。本実施形態では、上述した構成の自己位置計測装置である移動体に、加速度センサが備えられている。そして、演算手段であるメインコンピュータ45などが、移動体自体の加速度を計測し、また、撮影した画像中における他の物体の加速度を測定し、これらを比較することで、移動している他の物体を検出し、当該他の移動物体を特徴点から除く、という処理を行う機能を有している。例えば、移動体1の加速度との差が、あらかじめ定められた許容値よりも大きい場合には、画像中の移動物体上の所定領域を特徴点抽出領域から除くこととする。
【0164】
これにより、取得画像中の他の移動物体が特徴点となることを抑制して、位置計測の精度向上を図ることができると共に、そのような移動物体が存在する場所においても、本発明である移動体にて自己位置を計測することができ、汎用性の向上を図ることができる。
【0165】
〈第9の実施形態〉
次に、本発明の第9の実施形態について説明する。当該第9の実施形態においては、自己位置計測装置は人間が常時身につける携帯型である。例えば、登山者の帽子に装着可能な装置であって、かかる装置にステレオカメラ、傾斜角センサ、CPUなどが備えられている。そして、ステレオカメラなどで取得したデータは、無線通信手段を介して遠隔地のコンピュータに送信され、かかるコンピュータにて処理されてもよい。
【0166】
このようにすることにより、移動する人間の移動経路を計測したり、現在の位置を他の者が把握することが容易となる。そして、かかる測定値は、例えば、登山者が遭難したときの救助に利用することができる。
【0167】
【発明の効果】
本発明は、上記のように構成され機能するので、これによると、移動前後における周囲の少なくとも一つの特徴点の相対位置座標と、自己の姿勢角の変化情報とを測定し、これらをあらかじめ定められた式に代入して演算することにより、移動前の位置に対する移動後の自己位置を計測することができるため、安定かつ高精度な移動経路計測及び位置計測を行うことができる、という従来にない優れた効果を有する。特に、従来の少なくとも4つの特徴点から自己位置を計測する方法に対しては、安定性の面でその効果が顕著である。
【0168】
また、特徴点を複数抽出して、それぞれの特徴点について自己位置の計測を行い、それらの統計から総合的に自己位置を決定するという構成とすることで、さらなる計測値の精度向上を図ることができる。
【0169】
さらに、複数の特徴点から求めた自己位置にそれぞれ重みを付けて最終的な自己位置を決定する構成とすると共に、信頼性が高いと考えられる特徴点からの自己位置に付ける重みを大きく設定することで、周囲に他の移動物体が存在した場合であっても、かかる物体の影響を抑制することができ、より精度の高い自己位置の計測を実現できる。
【図面の簡単な説明】
【図1】本発明の第1の実施形態における動作の様子を示す概略図である。図1(a)は、時刻tの時の様子を示す図であり、図1(b)は、時刻t+1の時の様子を示す図である。
【図2】本発明の第1の実施形態における構成を示すブロック図である。
【図3】本発明の第1の実施形態における自己位置を計測する手順の概略を示すフローチャートである。
【図4】第1の実施形態における画像処理時の動作を示すフローチャートである。
【図5】図5(a)〜(h)は、第1の実施形態における画像の処理状態を示す説明図である。
【図6】図6(a)〜(d)は、第1の実施形態において、特徴点を抽出する際の処理状態を示す説明図である。
【図7】本発明の第2の実施形態における動作の様子を示す概略図である。図2(a)は、時刻tの時の様子を示す図であり、図2(b)は、時刻t+1の時の様子を示す図である。
【図8】図8(a)〜(d)は、本発明の第3の実施形態における画像処理領域の具体例を示す説明図である。
【図9】本発明の第10の実施形態において用いる他の移動物体を示す概略図である。
【図10】本発明の第10の実施形態における計測実験の一例の様子を示す図である。図10(a)は、実験の様子を示す説明図であり、図10(b)は、計測結果を示す図である。
【図11】本発明の第11の実施形態における計測実験の一例の様子を示す図である。図11(a)は、実験の様子を示す説明図であり、図11(b)は、計測結果を示す図である。
【図12】本発明の第12の実施形態における計測実験の一例の様子を示す図である。図12(a)は、実験の様子を示す説明図であり、図12(b)は、計測結果を示す図である。
【符号の説明】
1 移動体
2 ステレオカメラ(位置測定手段)
3 姿勢角センサ(姿勢検出手段)
10 メイン処理部
41 画像処理部(位置測定手段、演算手段)
42 CPU(演算手段)
45 メインコンピュータ(演算手段)
Claims (14)
- 装置自体の移動前に対する移動後の自己位置を計測する方法であって、
移動前に,装置自体に備えられた位置測定手段にて周囲の少なくとも一つの特徴点の相対位置を測定すると共に,装置自体に備えられた姿勢検出手段にて装置自身の姿勢情報を検出する移動前工程と、
移動後に,前記位置測定手段にて前記特徴点の相対位置を測定すると共に,前記姿勢検出手段にて装置自体の姿勢情報を検出する移動後工程と、
装置自体に備えられた演算手段を用いて,前記検出した姿勢情報から装置自身の移動前後における姿勢変化量である姿勢変化情報を算出すると共に,当該姿勢変化情報と装置の移動前後における前記特徴点の各相対位置情報とに基づいて装置自体の移動前に対する移動後の自己位置を算出する演算工程と、
を備えたことを特徴とする自己位置計測方法。 - 前記移動前工程は、前記位置測定手段にてあらかじめ定められた判断基準に基づいて周囲の任意の箇所を特徴点として選定することを特徴とする請求項1記載の自己位置計測方法。
- 前記移動前工程は、前記位置測定手段にてあらかじめ定められた判断基準に基づいて一つの特徴点を選定すると共に、
前記移動後工程は、前記移動前工程にて選定した特徴点と同一の特徴点を抽出する、ことを特徴とする請求項2記載の自己位置計測方法。 - 前記移動前工程は、あらかじめ定められた判断基準に基づいて複数の任意の特徴点を選定すると共に,それぞれの特徴点の相対位置を測定し、
前記移動後工程は、前記移動前工程にて選定した複数の特徴点と同一の特徴点をそれぞれ抽出して各特徴点の相対位置を測定し、
前記演算工程は、前記複数の特徴点毎に装置自体の移動前に対する移動後の自己位置を計測して,これら複数の計測値に基づいて最終的な自己位置を算出することを特徴とする請求項2記載の自己位置計測方法。 - 前記移動前工程及び前記移動後工程は、ステレオ視可能な複数のカメラによって構成された前記位置測定手段を用いて,前記各カメラによる複数の取得画像をマッチングすることにより各画像毎にそれぞれ対応する前記各特徴点を検出し,ステレオ視の原理により当該各特徴点の相対位置を測定すると共に、
前記移動後工程は、前記各カメラにて取得した複数の画像をマッチングする際に算出する当該マッチングの一致度に関する値を,前記各特徴点毎に対応付けて前記位置測定手段あるいは前記演算手段に記憶し、
前記演算工程は、前記複数の特徴点毎に計測した装置自体の移動前後における自己位置の各計測値に,前記各特徴点の一致度に関する値に応じた重みを付けて最終的な自己位置を算出する、ことを特徴とする請求項4記載の自己位置測定方法。 - 前記演算工程は、前記一致度が高くなるにつれて前記各計測値に付ける重みが大きくなるよう設定する、ことを特徴とする請求項5記載の自己位置測定方法。
- 前記移動前工程及び前記移動後工程は、画像取得手段にて構成された前記位置測定手段を用いて,取得画像の濃淡値に基づいて前記複数の特徴点を選定及び検出し,当該各特徴点の相対位置を測定すると共に、
前記移動前工程は、前記各特徴点を選定する際に算出する当該各特徴点の濃淡値に関する情報である濃淡特徴量を,当該各特徴点に対応付けて前記位置測定手段あるいは前記演算手段に記憶し、
前記演算工程は、前記複数の特徴点毎に計測した装置自体の移動前後における自己位置の各測定値に,前記各特徴点の濃淡特徴量に応じた重みを付けて最終的な自己位置を算出する、ことを特徴とする請求項4記載の自己位置測定方法。 - 前記移動前工程は、前記濃淡特徴量を前記画像上の所定領域内における最大濃淡値と最小濃淡値との差で表すと共に、
前記演算工程は、前記濃淡特徴量が大きくなるにつれて前記各計測値に付ける重みが大きくなるよう設定する、ことを特徴とする請求項7記載の自己位置測定方法。 - 前記移動後工程及び前記演算工程は、前記演算工程によって装置自体の移動前後における自己位置が算出された後に繰り返し作動すると共に,その際に,直前に作動した前記移動後工程にて抽出された前記複数の特徴点を前記移動前工程にて選定した複数の特徴点とみなして作動し、
前記移動後工程は、前記各特徴点を抽出する際に,当該各特徴点を連続して検出した回数をカウントすると共に,当該回数の値を前記各特徴点毎に前記位置測定手段あるいは前記演算手段に記憶し、
前記演算工程は、前記自己位置の測定を複数回行った後に,前記複数の特徴点毎に計測した装置自体の移動前後における自己位置の各測定値に,前記各特徴点の検出回数に応じた重みを付けて最終的な自己位置を算出する、ことを特徴とする請求項4記載の自己位置測定方法。 - 前記演算工程は、前記検出回数が多くなるにつれて前記各計測値に付ける重みが大きくなるよう設定する、ことを特徴とする請求項9記載の自己位置測定方法。
- 前記演算工程は、算出した装置自体の移動前後における最終的な自己位置に関する値を前記演算手段に記憶し、
前記移動後工程及び前記演算工程は、前記演算工程によって装置自体の移動前後における自己位置が算出された後に繰り返し作動すると共に,その際に,直前に作動した前記移動後工程にて抽出された前記複数の特徴点を前記移動前工程にて選定した複数の特徴点とみなして作動し、
前記演算工程は、自己位置の測定を複数回行った後に、
これまでの自己位置に関する値に基づいて装置自体の移動後の自己位置を予測して予測自己位置を算出する予測値算出工程と、
前記複数の特徴点毎に計測した自己位置の各測定値と前記予測自己位置とを比較して,当該各特徴点毎にそのずれ量を算出する比較工程と、
前記複数の特徴点毎に計測した自己位置の各測定値に,前記各特徴点毎に算出した前記ずれ量に応じた重みを付けて最終的な自己位置を算出する最終自己位置算出工程と、
を備えたことを特徴とする請求項4記載の自己位置測定方法。 - 前記演算工程は、前記ずれ量が小さくなるにつれて前記各計測値に付ける重みが大きくなるよう設定する、ことを特徴とする請求項11記載の自己位置測定方法。
- 装置自体の移動前に対する移動後の自己位置を計測する装置であって、
装置周囲の少なくとも一つの特徴点の相対位置を測定する位置測定手段と、装置自身の姿勢情報を検出する姿勢検出手段と、これらの動作を制御して取得した情報を演算処理する演算手段とを備え、
前記位置測定手段が、装置の移動前後におけるそれぞれの前記特徴点の相対位置を測定すると共に、前記姿勢検出手段が、装置自体の移動前後におけるそれぞれの姿勢情報を取得し、
前記演算手段が、前記取得した姿勢情報の移動前後における変化量である姿勢変化情報を算出すると共に、当該姿勢変化情報と装置自体の移動前後における前記特徴点の各相対位置情報とに基づいて当該装置自体の移動前に対する移動後の自己位置を算出する機能を有することを特徴とする自己位置計測装置。 - 装置自体の周囲画像を取得する画像取得手段と、装置自身の姿勢情報を検出する姿勢検出手段と、前記画像及び検出した姿勢情報を処理する演算手段とを備え、装置自体の移動前の対する移動後の自己位置を計測する装置であって、
前記演算手段が、前記画像取得手段にて取得した画像を用いて周囲画像中の少なくとも一つの特徴点の相対位置を当該装置自身の移動前後においてそれぞれ測定する特徴点位置測定機能と、前記姿勢検出手段にて検出した当該装置自身の移動前後における姿勢情報からその変化量を算出する姿勢変化情報算出機能と、装置自体の移動前後の前記特徴点の各位置情報と前記姿勢情報変化量とを予め定められた式に代入することにより装置自体の移動量を算出する移動量算出機能と、を有し、
これにより装置自体の移動前に対する移動後の自己位置を計測することを特徴とする自己位置計測装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2003144065A JP4046186B2 (ja) | 2003-02-03 | 2003-05-21 | 自己位置計測方法及び装置 |
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2003025790 | 2003-02-03 | ||
JP2003144065A JP4046186B2 (ja) | 2003-02-03 | 2003-05-21 | 自己位置計測方法及び装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2004294421A true JP2004294421A (ja) | 2004-10-21 |
JP4046186B2 JP4046186B2 (ja) | 2008-02-13 |
Family
ID=33421288
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2003144065A Expired - Fee Related JP4046186B2 (ja) | 2003-02-03 | 2003-05-21 | 自己位置計測方法及び装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP4046186B2 (ja) |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2007303994A (ja) * | 2006-05-12 | 2007-11-22 | Denso Corp | 外観検査装置及び外観検査方法 |
JP2008089314A (ja) * | 2006-09-29 | 2008-04-17 | Topcon Corp | 位置測定装置及びその方法 |
JP2008249407A (ja) * | 2007-03-29 | 2008-10-16 | Canon Inc | 画像処理装置、画像処理方法 |
JP2010100425A (ja) * | 2008-10-27 | 2010-05-06 | Mitsubishi Electric Corp | エレベータのドア制御装置 |
JP2011239310A (ja) * | 2010-05-13 | 2011-11-24 | Hitachi Ltd | 自由に設置可能なネットワークカメラ装置 |
JP2012169844A (ja) * | 2011-02-14 | 2012-09-06 | Sony Corp | 撮像装置、情報処理装置、及び撮像領域の共有判定方法 |
US9020196B2 (en) | 2012-08-15 | 2015-04-28 | International Business Machines Corporation | Extracting feature quantities from an image to perform location estimation |
-
2003
- 2003-05-21 JP JP2003144065A patent/JP4046186B2/ja not_active Expired - Fee Related
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2007303994A (ja) * | 2006-05-12 | 2007-11-22 | Denso Corp | 外観検査装置及び外観検査方法 |
JP2008089314A (ja) * | 2006-09-29 | 2008-04-17 | Topcon Corp | 位置測定装置及びその方法 |
JP2008249407A (ja) * | 2007-03-29 | 2008-10-16 | Canon Inc | 画像処理装置、画像処理方法 |
JP2010100425A (ja) * | 2008-10-27 | 2010-05-06 | Mitsubishi Electric Corp | エレベータのドア制御装置 |
JP2011239310A (ja) * | 2010-05-13 | 2011-11-24 | Hitachi Ltd | 自由に設置可能なネットワークカメラ装置 |
JP2012169844A (ja) * | 2011-02-14 | 2012-09-06 | Sony Corp | 撮像装置、情報処理装置、及び撮像領域の共有判定方法 |
US9020196B2 (en) | 2012-08-15 | 2015-04-28 | International Business Machines Corporation | Extracting feature quantities from an image to perform location estimation |
US9087268B2 (en) | 2012-08-15 | 2015-07-21 | International Business Machines Corporation | Extracting feature quantities from an image to perform location estimation |
Also Published As
Publication number | Publication date |
---|---|
JP4046186B2 (ja) | 2008-02-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111837083B (zh) | 信息处理装置、信息处理方法和存储介质 | |
US20210201509A1 (en) | Monitoring method and device for mobile target, monitoring system and mobile robot | |
CN110312912B (zh) | 车辆自动停车系统和方法 | |
JP4672175B2 (ja) | 位置検出装置、位置検出方法、及び位置検出プログラム | |
EP3347875B1 (en) | Localization of a robot in an environment using detected edges of a camera image from a camera of the robot and detected edges derived from a three-dimensional model of the environment | |
EP3627446B1 (en) | System, method and medium for generating a geometric model | |
KR102457222B1 (ko) | 이동 로봇 및 그 제어 방법 | |
Kuramachi et al. | G-ICP SLAM: An odometry-free 3D mapping system with robust 6DoF pose estimation | |
EP1715455A1 (en) | Method and system for transforming 2D image domain data into a 3D dense range map | |
CN111055281A (zh) | 一种基于ros的自主移动抓取系统与方法 | |
US20100161225A1 (en) | Method of building map of mobile platform in dynamic environment | |
CN112740274A (zh) | 在机器人设备上使用光流传感器进行vslam比例估计的系统和方法 | |
JP2011022157A (ja) | 位置検出装置、位置検出方法、及び位置検出プログラム | |
CN115552348A (zh) | 移动目标跟随方法、机器人和计算机可读存储介质 | |
JP4967758B2 (ja) | 物体移動の検出方法及び検出装置 | |
JP4046186B2 (ja) | 自己位置計測方法及び装置 | |
JP7278740B2 (ja) | 移動体制御装置 | |
JP7354528B2 (ja) | 自律移動装置、自律移動装置のレンズの汚れ検出方法及びプログラム | |
CN111971149A (zh) | 记录介质、信息处理设备和信息处理方法 | |
JP7273696B2 (ja) | 位置推定装置及びシステム | |
JP7327596B2 (ja) | 自律移動装置、自律移動装置のレンズの汚れ検出方法及びプログラム | |
JP7249919B2 (ja) | 推定装置、推定方法及びプログラム | |
KR102682194B1 (ko) | 비상 상황 발생 시 공간 내의 요 구조자를 식별하는 로봇 및 로봇 제어 방법 | |
Wang et al. | Robot Visual Simultaneous Localization and Mapping in Dynamic Environments | |
JP2024088932A (ja) | 情報処理装置、制御方法及びプログラム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20051213 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20070809 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20070815 |
|
A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20071011 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20071106 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20071114 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20101130 Year of fee payment: 3 |
|
R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
LAPS | Cancellation because of no payment of annual fees |