JP4046186B2 - Self-position measuring method and apparatus - Google Patents

Description

【００５７】
【数２】

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

【００６１】
【数４】

【００６２】
【数５】

【００６３】
【数６】

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

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

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

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

【００９１】
【数１１】

【００９２】
【数１２】

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

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

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

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

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

【０１３７】
【数１８】

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

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

【０１４２】
【数２１】

【０１４３】
【数２２】

【０１４４】
【数２３】

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

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

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

【０１５０】
また、上式のように定義したのは、観測雑音の分散の不定性を、上式の右式の初期値の不定性の中にまとめて取り込むためである。ここで、雑音の分散比σ_ｗ ^２／σ_ｖ ^２が最終的に未知パラメータとしてカルマンフィルタに残る。
【０１５１】
以上より、システム雑音及び観測雑音の分散比σ_ｗ ^２，σ_ｖ ^２を決定すれば、移動量の予測値を含む状態予測ベクトルとして数２４式の上から２番目の式が、逐次得られる。なお、以下に説明する実施例では、上記分散比をトライアンドエラーにより決定する。
【０１５２】
以上のように、カルマンフィルタによって算出される移動量の予測値は、過去の移動量の時系列データをもとに算出されるため、計測開始時点で視界内に移動物体がなければ、算出される予測値にも移動物体の影響は入らない。そして、他の移動物体上の特徴点から算出した移動量は予測値から大きなずれが生じると考えられるため、複数得られた特徴点毎の移動量に対して予測値からのずれ量が大きいほど小さな荷重をかけ、一方、ずれ量が小さいほど大きな加重がかかるようなフィルタをかければ、他の移動物体の影響を抑制することができる。これにより、より制度の高い移動量を算出することができる。
【０１５３】
ここで、本実施形態では、統計の理論を応用し、予測値からのずれは、予測値を平均とする正規分布に従うと考え、実測値をもとに正規分布の分散σ^２を決定し、この正規分布関数の値を重みとして使用する。すなわち、各特徴点から計測された移動量が予測値と同一の場合に重みを１とし、当該予測値から離れるにつれて０に近づくよう、重みを設定する。なお、正規分布関数は、以下に説明するように、トライアンドエラーにより適切に設定し、かかる分布をメインコンピュータ４５や移動体１内のＣＰＵや所定の記憶部に記憶保持しておく。
【０１５４】
（実施例）
ここで、上述したように、カルマンフィルタを用いて移動体１の移動量の予測値を算出し、当該予測値に基づいた重みを使用して、複数の特徴点から自己位置を計測した実験を説明する。この実験では、図１乃至図２に示す自己位置計測システムが搭載された移動体１を前進走行させて、その移動体１のカメラ視界内で他の移動物体１００を移動させ、かかる環境下で移動体１の移動量を計測する。そして、上記カルマンフィルタの有効性を検証するというものである。
【０１５５】
ここで、かかる実験では、移動体１の本システムにより計測した移動量と比較する実際の移動量の計測（デッドレコニング）も必要になる。これについては、車輪の回転数とステア角をロータリエンコーダで測定して、これを積分して位置計測を行う手法であるオドメトリによる位置推定を行い、かかる計測値を理論値として取り扱う。なお、実験環境では、移動体１が前進走行する床は平面であり、滑りも起こりにくいため、直進移動距離の計測は誤差率１％以内である。
【０１５６】
また、実験で使用する他の移動物体１００は、自己位置を計測する移動体１とは異なる動きをするものであり、その構成の概要を図９に示す。この図に示すように、他の移動物体１００は、シャシ１０１に図示しない駆動源により駆動される自走用の車輪１０２を備え、また、移動体１が認識しやすい特徴点となりうる黒いフロックボール１０３を４つ高い位置に備えている。
【０１５７】
さらに、上述した数２４式の上から第４式に示すシステム雑音及び観測雑音の分散比σ_ｗ ^２，σ_ｖ ^２、及び、測定値と予測値とのずれの分散σ^２を決定する必要がある。ここで、σ_ｖ ^２については、あらかじめ測定でき、一方、σ_ｗ ^２についてはトライアンドエラーにより決定した。また、σ^２は、σ_ｗ ^２，σ_ｖ ^２を決定した後に測定した値をもとにトライアンドエラーを行って調節した。そして、これらの値は、各数式に代入されたり、あるいは、自己位置を計測する際に参照できるよう、メインコンピュータ４５や移動体１内のＣＰＵや記憶部などに記憶されている。
【０１５８】
このようにして、実際に実験を行ったときの様子を、図１０乃至図１３に示す。ここで、図１０（ａ）に示す例は、自己位置計測装置自体である移動体１を、壁２００に向かって静止させた状態で、他の移動物体１００を矢印Ａ１００に示すように、ラインＬ１００に沿って移動させる場合である。このとき、移動体１の前方には、種々の特徴点となりうる物体（図示せず）が存在しており、また、壁２００には、特徴点となる図形が描かれている。
【０１５９】
そして、図１０（ｂ）に、移動体１の自己位置の計測結果を示す。横軸には計測時間ｔ［ｓ（秒）］をとり、縦軸には移動体１の壁方向２００への移動距離ｔｘ［ｍｍ］をとる。この図に示すように、静止している移動体１において、その計測値はほぼ０の値を示している。すなわち、静止時には他の移動物体１００の影響をほぼ受けずに精度の高い自己位置の計測が可能である。なお、かかる実験においては、移動量として壁２００方向への距離ｘのみを計測しているが、これに限定されない。もちろん、上述してきたように、三次元（ｘ，ｙ，ｚ）における移動量も測定可能である。
【０１６０】
次に、図１１（ａ）に示すように、他の移動物体１００を矢印Ａ１００方向に、ラインＬ１００に沿って移動させ、また、移動体１を矢印Ａ１方向に前進させた場合を説明する。具体的には、自己位置を計測する移動体１が、他の移動物体１００に追い越される環境において計測を行った。そして、そのときの自己位置の計測結果を図１１（ｂ）に示す。この図では、図１０（ｂ）と同様に、横軸には計測時間ｔ［ｓ（秒）］をとり、縦軸には移動体１の移動距離ｔｘ［ｍｍ］をとっており、デッドレコニングによる移動量の理論値（実線）と、本システムによる移動量の計測値（菱形プロット）を示している。すると、当該計測値はほぼ理論値と差がなく、高精度な自己位置の計測が可能であることがわかる。
【０１６１】
さらに、図１２（ａ）に示すように、他の移動物体１００を矢印Ａ１０１方向に、ラインＬ１０１に沿って移動させ、また、移動体１を矢印Ａ１方向に前進させた場合を説明する。具体的には、自己位置を計測する移動体１が、他の移動物体１００とすれ違う環境において計測を行った。そして、そのときの自己位置の計測結果を図１２（ｂ）に示す。そして、この図には、デッドレコニングによる移動量の理論（実線）値と、本システムによる移動量の計測値（菱形プロット）とを示したが、このような実験例においても、当該計測値はほぼ理論値と差がなく、高精度な自己位置の計測が可能であることがわかる。
【０１６２】
以上のように、予測値に基づくフィルタを用いて自己位置の計測を行うことで、カメラの視界内に他の移動物体１００が存在している場合であっても、当該他の移動物体１００の存在の影響を抑制して、高精度な自己位置計測を実現できる。すなわち、他の移動物体１００を特徴点として追従した場合であっても、その影響を軽減して、自己位置を計測することができる。
【０１６３】
〈第８の実施形態〉
次に、本発明の第８の実施形態について説明する。本実施形態では、上述した構成の自己位置計測装置である移動体に、加速度センサが備えられている。そして、演算手段であるメインコンピュータ４５などが、移動体自体の加速度を計測し、また、撮影した画像中における他の物体の加速度を測定し、これらを比較することで、移動している他の物体を検出し、当該他の移動物体を特徴点から除く、という処理を行う機能を有している。例えば、移動体１の加速度との差が、あらかじめ定められた許容値よりも大きい場合には、画像中の移動物体上の所定領域を特徴点抽出領域から除くこととする。
【０１６４】
これにより、取得画像中の他の移動物体が特徴点となることを抑制して、位置計測の精度向上を図ることができると共に、そのような移動物体が存在する場所においても、本発明である移動体にて自己位置を計測することができ、汎用性の向上を図ることができる。
【０１６５】
〈第９の実施形態〉
次に、本発明の第９の実施形態について説明する。当該第９の実施形態においては、自己位置計測装置は人間が常時身につける携帯型である。例えば、登山者の帽子に装着可能な装置であって、かかる装置にステレオカメラ、傾斜角センサ、ＣＰＵなどが備えられている。そして、ステレオカメラなどで取得したデータは、無線通信手段を介して遠隔地のコンピュータに送信され、かかるコンピュータにて処理されてもよい。
【０１６６】
このようにすることにより、移動する人間の移動経路を計測したり、現在の位置を他の者が把握することが容易となる。そして、かかる測定値は、例えば、登山者が遭難したときの救助に利用することができる。
【０１６７】
【発明の効果】
本発明は、上記のように構成され機能するので、これによると、移動前後における周囲の少なくとも一つの特徴点の相対位置座標と、自己の姿勢角の変化情報とを測定し、これらをあらかじめ定められた式に代入して演算することにより、移動前の位置に対する移動後の自己位置を計測することができるため、安定かつ高精度な移動経路計測及び位置計測を行うことができる、という従来にない優れた効果を有する。特に、従来の少なくとも４つの特徴点から自己位置を計測する方法に対しては、安定性の面でその効果が顕著である。
【０１６８】
また、特徴点を複数抽出して、それぞれの特徴点について自己位置の計測を行い、それらの統計から総合的に自己位置を決定するという構成とすることで、さらなる計測値の精度向上を図ることができる。
【０１６９】
さらに、複数の特徴点から求めた自己位置にそれぞれ重みを付けて最終的な自己位置を決定する構成とすると共に、信頼性が高いと考えられる特徴点からの自己位置に付ける重みを大きく設定することで、周囲に他の移動物体が存在した場合であっても、かかる物体の影響を抑制することができ、より精度の高い自己位置の計測を実現できる。
【図面の簡単な説明】
【図１】本発明の第１の実施形態における動作の様子を示す概略図である。図１（ａ）は、時刻ｔの時の様子を示す図であり、図１（ｂ）は、時刻ｔ＋１の時の様子を示す図である。
【図２】本発明の第１の実施形態における構成を示すブロック図である。
【図３】本発明の第１の実施形態における自己位置を計測する手順の概略を示すフローチャートである。
【図４】第１の実施形態における画像処理時の動作を示すフローチャートである。
【図５】図５（ａ）〜（ｈ）は、第１の実施形態における画像の処理状態を示す説明図である。
【図６】図６（ａ）〜（ｄ）は、第１の実施形態において、特徴点を抽出する際の処理状態を示す説明図である。
【図７】本発明の第２の実施形態における動作の様子を示す概略図である。図２（ａ）は、時刻ｔの時の様子を示す図であり、図２（ｂ）は、時刻ｔ＋１の時の様子を示す図である。
【図８】図８（ａ）〜（ｄ）は、本発明の第３の実施形態における画像処理領域の具体例を示す説明図である。
【図９】本発明の第１０の実施形態において用いる他の移動物体を示す概略図である。
【図１０】本発明の第１０の実施形態における計測実験の一例の様子を示す図である。図１０（ａ）は、実験の様子を示す説明図であり、図１０（ｂ）は、計測結果を示す図である。
【図１１】本発明の第１１の実施形態における計測実験の一例の様子を示す図である。図１１（ａ）は、実験の様子を示す説明図であり、図１１（ｂ）は、計測結果を示す図である。
【図１２】本発明の第１２の実施形態における計測実験の一例の様子を示す図である。図１２（ａ）は、実験の様子を示す説明図であり、図１２（ｂ）は、計測結果を示す図である。
【符号の説明】
１ 移動体
２ ステレオカメラ（位置測定手段）
３ 姿勢角センサ（姿勢検出手段）
１０ メイン処理部
４１ 画像処理部（位置測定手段、演算手段）
４２ ＣＰＵ（演算手段）
４５ メインコンピュータ（演算手段）[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a self-position measuring method and apparatus, and more particularly, to a method and apparatus for measuring a self-position after movement relative to a position before movement of a movable body or a portable apparatus itself.
[0002]
[Prior art]
Conventionally, methods for measuring the self-position of the device itself have been studied, and devices that realize this have been developed. And it can be applied to moving objects themselves or objects carried by humans, etc., measuring the position after moving, and connecting the positions to measure the movement path and current position of the device after a certain period of time it can.
[0003]
Here, as a self-position measuring method conventionally used, there is a method using an image. A system that measures the self-position based only on such an image needs to always accurately and stably measure the relative position (or the amount of change in the relative position) of at least four points on the environment. However, it is very difficult to measure all four positions at all times in a stable manner. In reality, when the number of points is not more than three, the function stops at that point. Even if four points can be measured, there is a problem in that the accuracy of the measurement position is severely degraded if the measurement accuracy is low even at one point.
[0004]
In this method, since the self-position is obtained by triangulation from the coordinates of the four points, the four points are necessarily separated from each other. Therefore, if the four points are reflected in the vicinity of the periphery of the screen, any of the four points will be immediately removed from the screen if the camera moves. This means that it is structurally difficult to continuously measure four points.
[0005]
In order to overcome this problem, it was necessary to use a plurality of cameras or a special wide-angle lens. However, using a plurality of camera sets complicates the system and makes it difficult to maintain high accuracy between cameras. Further, when a wide-angle lens is used, there arises a problem that accuracy is lowered. As another problem, it is difficult to use a moving object in a measurement image.
[0006]
And in order to solve the problem of the said prior art, the technique as shown to patent document 1 is also disclosed. The technique shown in Patent Document 1 is to capture a peripheral image during traveling, measure the self-position based on the image of the traveling peripheral environment, and move toward the target. The self-position mounted on the self-propelled vehicle It is a measuring device. In this apparatus, a plurality of feature objects whose positions in the real space are known are detected on the image, and the self-position is obtained based on the position of each feature in the real space and the position in the image plane. .
[0007]
[Patent Document 1]
JP-A-9-53939
[0008]
[Problems to be solved by the invention]
However, the prior art described in Patent Document 1 has the following disadvantages. That is, the one in Patent Document 1 has a problem in that the position of the feature in the real space must be stored in the device in advance, and the self-position cannot be measured outside a specific place.
[0009]
In addition, as described above, the method of extracting four feature points on an image is difficult to maintain the accuracy of self-position measurement. There is a problem.
[0010]
OBJECT OF THE INVENTION
The object of the present invention is to provide a method and an apparatus that can improve the disadvantages of the above-described conventional example, and can measure the self-position in a stable and accurate manner in any place.
[0011]
[Means for Solving the Problems]
Therefore, in the present invention, a method of measuring the self-position after movement relative to the movement of the apparatus itself, and before the movement, relative position of at least one feature point around by the position measuring means provided in the apparatus itself And measuring the relative position of the feature point by the position measuring means after the movement, and detecting the posture of the apparatus itself by the attitude detecting means provided in the apparatus itself. And a post-movement process for detecting posture information of the apparatus itself. Then, following the post-movement process, the posture change information, which is the amount of posture change before and after the movement of the device itself, is calculated from the detected posture information using the calculation means provided in the device itself, and the posture change information and A configuration is adopted in which a calculation step of calculating a self-position after the movement of the device itself with respect to the movement of the device itself based on the relative position information before and after the movement of the feature point is provided.
[0012]
By adopting the configuration according to such a method, first, the relative position of at least one feature point with respect to the apparatus is measured by the position measuring means before the movement, and the attitude information of the apparatus itself is detected by the attitude detecting means. After the movement, the relative position of the same feature point is measured to detect the posture information of the device itself. After that, the calculation means substitutes each relative position information of the feature points before and after the movement and its own posture change information into a predetermined formula to calculate the position after the movement of the device itself relative to the position before the movement. Calculate the self-position of. Accordingly, since the self-position can be measured by measuring the position information of at least one feature point and the posture information, the position can be measured easily and stably, and the measurement accuracy can be improved. be able to.
[0013]
Further, it is desirable that the pre-movement process has a configuration in which an arbitrary location around is selected as a feature point based on a criterion determined in advance by the position measuring means. Accordingly, the self-position can be measured by selecting an arbitrary feature point in any environment, and the adaptability can be improved.
[0014]
In addition, the pre-movement process selects one feature point based on a criterion determined in advance by the position measuring means, and the post-movement process uses the same feature point as the feature point selected in the pre-movement process. The structure of extracting may be sufficient. As a result, as described above, the self-position can be measured even with one feature point. Therefore, the information amount of the measurement process can be reduced and the processing load of the calculation means can be suppressed while realizing the measurement. . In addition, the measurement process can be speeded up and real-time processing of self-position measurement is realized.
[0015]
Furthermore, in the pre-movement process, a plurality of arbitrary feature points are selected based on a predetermined criterion, and the position of each feature point is measured, and the post-movement process is selected in the pre-movement process. The feature points that are the same as a plurality of feature points are extracted to measure the position of each feature point, and the calculation step measures the self-position after movement relative to the movement of the device itself for each of the plurality of feature points, It is even more desirable if the final self-position is calculated based on the plurality of measurement values.
[0016]
Thereby, it is possible to further improve the measurement accuracy by selecting a plurality of feature points, measuring the self-position for each feature point, and measuring the final self-position by combining these measurement results. In addition, by selecting a plurality of feature points, even if feature points that cannot be extracted after movement are generated, the self-position can be measured from other feature points, so that stable measurement can be performed.
[0017]
Further, in the method for measuring the self-position using the plurality of feature points, the pre-movement process and the post-movement process may be performed by using a plurality of position measuring means configured by a plurality of stereo-viewable cameras. Each feature point corresponding to each image is detected by matching the acquired images, and the relative position of each feature point is measured based on the principle of stereo vision. A value relating to the degree of matching of the matching calculated when matching images is stored in the position measuring means or the calculating means in association with each feature point, and the calculation step is performed by the apparatus itself measured for each of the plurality of feature points. It is desirable that the final self-position be calculated by applying a weight according to the value related to the degree of coincidence of each feature point to each measured value of the self-position before and after moving There. At this time, it is more preferable that the calculation process is set so that the weight given to each measurement value increases as the matching degree at the time of matching increases.
[0018]
By doing in this way, when calculating the final self-position by combining a plurality of obtained self-position measurement values for each feature point, the device calculated from the feature points that could be detected accurately A large weight is added to the self-position measurement value, and the final self-position is measured with more importance on the measurement value. Therefore, the self-position with higher reliability can be calculated, and the measurement system can be improved.
[0019]
In addition, when measuring using a plurality of feature points as described above, a plurality of pre-movement steps and post-movement steps are performed based on the gray value of the acquired image using the position measurement unit configured by the image acquisition unit. Is selected and detected, and the relative position of each feature point is measured, and the pre-movement process is a shade feature value that is information about the shade value of each feature point calculated when each feature point is selected. Is stored in the position measurement means or calculation means in association with each feature point, and the calculation step calculates the value of each feature point for each measured value of its own position before and after the movement of the device itself measured for each of the plurality of feature points. It is desirable that the final self-position is calculated by applying a weight according to the density feature amount. At this time, the pre-movement process represents the density feature value by the difference between the maximum density value and the minimum density value in a predetermined area on the image, and the calculation process includes a weight to be given to each measurement value as the density feature quantity increases. It is even more desirable to set the value to be larger.
[0020]
By doing so, a large weight is added to the measured value of the self-position of the device calculated from the characteristic points having a large shade difference, and the final self-position is determined with more importance on the measured value. Will be measured. Therefore, the self-position with higher reliability can be calculated, and the measurement system can be improved.
[0021]
Further, when measuring using a plurality of feature points as described above, the post-movement process and the calculation process are repeatedly operated after the self-position before and after the movement of the apparatus itself is calculated by the calculation process, and at that time, The multiple feature points extracted in the post-movement process that was activated immediately before are considered to be the multiple feature points selected in the pre-movement process, and the post-movement process operates when extracting each feature point. Count the number of times feature points have been detected continuously, and store the value of the number of times in the position measurement means or calculation means for each feature point. It is desirable that the final self-position is calculated by assigning a weight corresponding to the number of detections of each feature point to each measured value of the self-position before and after the movement of the device itself measured for each of a plurality of feature points. . At this time, it is more desirable that the calculation process is set so that the weight given to each measurement value increases as the number of detections increases.
[0022]
In this way, in a configuration that continuously tracks feature points and also measures the movement path of the device, the device self-position is calculated from highly reliable feature points that are continuously detected. A large weight is added to the value, and the final self-position is measured with more importance on the measured value. Therefore, the self-position with higher reliability can be calculated, and the measurement system can be improved.
[0023]
Further, in the configuration in which measurement is performed using a plurality of feature points as described above, and the movement path of the device is also measured by continuously following the feature points, the calculation step is performed at the final stage before and after the movement of the calculated device itself. A value related to the actual self-position is stored in the calculation means, and the calculation step performs a self-position measurement several times, and then predicts the self-position after the movement of the device itself based on the value related to the self-position. A predicted value calculation step for calculating a predicted self-position, and a comparison step for comparing each measured value of the self-position measured for each of a plurality of feature points with the predicted self-position and calculating a deviation amount for each of the feature points And a final self-position calculation step of calculating a final self-position by applying a weight according to the amount of deviation calculated for each feature point to each measured value of the self-position measured for each of the plurality of feature points. It is desirable that the configuration is provided. At this time, it is desirable that the calculation process is set so that the weight assigned to each measurement value increases as the deviation amount decreases.
[0024]
In this way, first, a predicted value calculated by a predetermined calculation method based on the past movement route is calculated. Then, the predicted value is compared with the self-position calculated for each feature point, and a large weight is added to the measured value of the self-position with a small deviation amount, which is the difference between the predicted value and the measured value is more emphasized, and finally Measure self-position. Therefore, the influence of a value with a large deviation amount and a low possibility as the next self-position can be suppressed, a self-position with higher reliability can be calculated, and the measurement system can be improved.
[0025]
Further, in the present invention, an apparatus for measuring a self-position after movement relative to the movement of the apparatus itself, the position measuring means for measuring the relative position of at least one feature point around the apparatus, and the attitude information of the apparatus itself. A position detecting means for measuring the relative position of each feature point before and after the movement of the device, and a posture measuring means for calculating and processing information obtained by controlling these operations. The detection means acquires each posture information before and after the movement of the device itself, and the calculation means calculates posture change information that is a change amount of the acquired posture information before and after the movement, and the posture change information and the device itself. Self-position with a configuration that has a function of calculating a self-position after movement relative to before the movement of the device itself based on each relative position information of feature points before and after movement Also it provides a measuring apparatus.
[0026]
More specifically, the image processing apparatus includes an image acquisition unit that acquires an image around the apparatus itself, an attitude detection unit that detects the attitude information of the apparatus itself, and a calculation unit that processes the image and the detected attitude information. An apparatus for measuring a self-position after movement relative to a position before the movement, wherein the calculation means calculates the relative position of at least one feature point in the surrounding image using the image acquired by the image acquisition means. Feature point position measurement function that measures before and after movement, posture change information calculation function that calculates the amount of change from posture information before and after the movement of the device itself detected by the posture detection means, and features before and after the movement of the device itself A movement amount calculation function for calculating the movement amount of the device itself by substituting each position information of the point and the change amount of the posture information into a predetermined formula, thereby moving the device itself Also it provides a self-position measuring apparatus that measures its own position after movement with respect.
[0027]
Even if such a configuration is adopted, the same operations and effects as described above can be obtained, and the above object can be achieved.
[0028]
DETAILED DESCRIPTION OF THE INVENTION
<First Embodiment>
Hereinafter, a first embodiment of the present invention will be described with reference to FIGS. 1 to 6. FIG. 1 is a schematic diagram showing an operation state of the self-position measuring apparatus in the present embodiment. FIG. 2 is a block diagram showing a configuration of the self-position measuring device. FIG. 3 is a flowchart showing a procedure for measuring the self-position of the apparatus. FIG. 4 is a flowchart showing an operation when image processing is performed in the apparatus. FIG. 5 to FIG. 6 are explanatory diagrams showing the state of image processing.
[0029]
(overall structure)
The self-position measuring device according to the present invention is provided in the vehicle-type moving body 1 in the present embodiment. And the self-position after a movement with respect to the position before the movement of mobile body 1 itself is measured. An outline of the configuration will be described first with reference to FIG.
[0030]
A moving body 1 shown in FIG. 1A includes position measuring means 2 for measuring a relative position of at least one feature point around the moving body 1 with respect to the moving body 1 and posture detection for detecting posture information of the moving body itself. Means (not shown) and calculation means (not shown) for calculating information obtained by controlling these operations are provided. Each of these components generally has the following functions.
[0031]
The position measuring means 2 extracts and tracks (tracks) feature points before and after the moving body 1 moves, and measures the relative position of the feature points with respect to the apparatus. The posture detection means acquires each posture information before and after the moving body itself moves. The computing means calculates posture change information, which is a change amount of the acquired posture information before and after movement, and based on the posture change information and each relative position information of feature points before and after movement of the moving body itself. The self-position after movement relative to the movement of the moving body itself is calculated.
[0032]
Here, in the present embodiment, specifically, a configuration as shown in FIG. 2 is adopted. First, the position measuring unit 2 is a stereo camera that is an image acquiring unit that acquires an image around the moving body 1 itself. As shown in FIG. 1A, the stereo camera 2 is fixed and provided at an upper end portion of a support column 12 extending upward from the chassis portion 11 of the moving body 1. In this embodiment, the stereo camera 2 is constituted by two CCD cameras, and an image captured from one camera is handled as a master image, and an image captured by the other camera is handled as a slave image. The stereo camera is not limited to being composed of two cameras.
[0033]
Further, the posture detection means for detecting the posture information of the moving body 1 is a posture angle sensor 3 for detecting the posture angle of the moving body 1. Although not shown in FIG. 1A, the posture angle sensor 3 is fixed to the chassis unit 11 of the moving body 1. Further, arithmetic processing units 41, 42, 43, 44, 45 which are arithmetic means for processing the image captured by the stereo camera 2 and the posture information detected by the posture angle sensor 3 are placed inside and outside the moving body 1. I have. This will be described in further detail below.
[0034]
(Moving body)
First, the outline of the configuration of the moving body 1 is shown in FIG. 1A. This moving body 1 is provided with wheels 13 and 14 (two wheels) on the left and right sides of the chassis portion 11, and is a vehicle that travels thereby. is there. For example, it is an unmanned moving body that searches the lunar surface. However, the configuration of the mobile body 1 is not limited to this, and may be a mobile body such as a general manned automobile.
[0035]
And in order to make this mobile body 1 run, DC motors 13a and 14a which drive each wheel 13a and 13b, encoders 13b and 14b which detect the rotation angle of each motor, and a detection value are read from each encoder, and it is from a high-order computer. The chassis unit 11 includes CPUs 43 and 44 as motor drivers that issue rotation control commands to the DC motors 13 a and 14 a based on the commands.
[0036]
(Stereo camera)
As described above, the stereo camera 2 is provided on the upper portion of the chassis 11 of the moving body 1 and captures a stereo parallax image around the moving body. The captured image is processed by the image processing unit 41 or the like as a computing means, and a part of the image is transmitted to the monitor 10b of the main processing unit 10 at a remote location via the transmitter 15 and displayed to the operator. In addition, the image processing unit 41 has a function of extracting a feature point from a surrounding image by operating a program based on a predetermined algorithm and calculating three-dimensional coordinates (body coordinate system) representing the position of the feature point. Have. That is, the image processing unit 41 as an image calculation unit measures the relative position of at least one feature point in the surrounding image using the image acquired by the image acquisition unit before and after the movement of the moving body 1 itself. It has a feature point position measurement function. Specific processing will be described in detail when the operation is described. Note that the configuration of the image acquisition unit that is the stereo camera 2 and the image processing unit 41 or the like (including the main computer 45) that processes the acquired image is referred to as a position measurement unit above.
[0037]
Then, the position information of the feature points measured from the images is transmitted to the main computer 45 of the main processing unit 10 located at a remote location via the hub 16 and the wireless LANs 17 and 10a, and the self position of the moving body 1 itself is calculated. Used when The main processing unit 10 will be described later.
[0038]
However, the position information of the feature point may be transmitted to another CPU 42 mounted on the moving body 1 via the hub 16 and the CPU 42 may calculate the self position of the moving body 1. Further, a series of processes such as a process of extracting feature points from an image may be executed by the main computer 45 of the main processing unit 10 located at a remote place. That is, the acquired image information itself may be transmitted from the hub 16 to the main computer 45 via the wireless LANs 17 and 10a. Even in this case, the main computer 45 is in a remote place, only the computer to process is different, and the configuration of the present invention is the same.
[0039]
(Attitude angle sensor)
The attitude angle sensor 3 is fixed to the chassis unit 11 of the moving body 1. In this embodiment, the inclination angle sensor and the azimuth meter are used as the attitude angle sensor 3. Get information. These pieces of information are acquired from the posture angle sensor 3 by the CPU 42 controlling the operation of the posture angle sensor 3, and thereafter transmitted to the main computer 45 via the hub 16 and the wireless LANs 17 and 10a. 45 is used for self-position calculation processing. However, the CPU 42 that has detected the information related to the posture angle such as the tilt angle may execute a process performed by the main computer 45 to be described later, that is, a process for calculating the amount of change compared with the value detected last time. This process will be described later.
[0040]
Incidentally, the apparatus according to the present invention can be configured at a relatively low cost by using the tilt angle sensor or the azimuth meter as the attitude angle sensor 3. However, the posture angle sensor 3 is not limited to this. Since it is only necessary to know the amount of change from the attitude angle information detected from the sensor 3, and it is not always necessary to detect the absolute angle of the moving body 1, a gyroscope or the like may be used. Specifically, triaxial gyro values may be integrated. When it is clear that the posture angle does not change, that is, when moving on a horizontal plane, only one axis of the azimuth angle needs to be measured.
[0041]
The CPU 42 also has a function of issuing a drive command to the other CPUs 43 and 44 that control the drive state of the left and right wheels 13 and 14 in response to a command from the main computer 45.
[0042]
(Main processing part)
Further, in the present embodiment, since it is an unmanned mobile body 1, a travel command from an operator is input, and the main processing unit 10 that controls the travel state of the mobile body 11 according to the travel command is separated from the mobile body 1. Is provided. Here, when the mobile body 1 is a self-propelled type, a control program for self-running may be executed to remotely control the travel state of the mobile body 1. However, the main processing unit 10 may be provided in the moving body 1 itself.
[0043]
The main processing unit 10 includes a main computer 45 having a CPU, a memory, and the like, and performs data communication with the mobile unit 1 through a communication unit such as a wireless LAN 10a. Further, the main processing unit 10 is provided with a monitor 10b that receives and displays an image captured and transmitted by the mobile body 1. Thereby, the operator can issue a travel command while confirming the situation around the moving body 1 on the monitor 10b.
[0044]
Here, the main computer 45 mainly calculates posture angle change information, which is a change amount before and after the movement of the moving body 1 itself, from the posture information detected by the posture angle sensor 3 and transmitted from the moving body 1. Movement for calculating the movement amount of the moving body 1 itself by substituting the posture change information calculation function, the position information of the feature points before and after the movement of the moving body 1 itself, and the amount of change in posture information into a predetermined formula. And a quantity calculation function.
[0045]
Specifically, with the posture angle change information, the roll angle, pitch angle, and yaw angle change amount is calculated from the orientation and inclination angle of the chassis unit 11 detected by the posture angle sensor 3. Then, in the movement amount calculation function, by using the information such as the roll angle and the position information of the feature points before and after the movement, by substituting into the calculation formula stored in the main computer 45 in advance, the moving body It is possible to obtain the three-dimensional coordinates after the movement relative to the one before the movement.
[0046]
Then, the main computer 45 stores information regarding the calculated moving amount of the moving body 1 and accumulates the moving amount thereafter, whereby the moving path and the current position of the moving body 1 can be calculated.
[0047]
Here, the main computer 45 may perform all the processes executed by the computing means 41, 42, 43, and 44 on the moving body 1 described above. In any case, there is no difference in the configuration whether it is processed by the computing means 41 on the mobile body 1 or the main computer 45 at a remote place.
[0048]
(Operation)
Next, basic operations in the first embodiment of the present invention will be described with reference to FIG. FIG. 3 is a flowchart showing the basic operation in the present embodiment. Note that the following operations are built in various functions by incorporating each operation program in advance in each processing unit (image processing unit 41, CPU 42, 43, 44, etc.) in the main computer 45 or the moving body 1. To be realized. Data necessary for realizing the following operation is also stored in the main computer 45 or the like in advance.
[0049]
The moving body 1 first acquires a surrounding image with the stereo camera 2 before starting the movement or at a certain timing during the movement. Then, the image captured by the image processing unit 41 is processed, and at least one feature point (one feature point in the present embodiment) is extracted from the surrounding image. Thereafter, the relative position from the moving body 1 to the extracted feature point is measured, and three-dimensional coordinates are calculated. At the same time, the posture angle sensor 3 detects posture information such as the direction of the moving body 1 itself and the inclination angle. These pieces of information are transmitted from the moving body 1 to the main processing unit 10 and stored and held in the main computer 45 (step S1, pre-movement process).
[0050]
Subsequently, even if the moving body 1 moves or does not move, when a predetermined sampling time elapses (affirmative determination in step S2), the surrounding image is captured again, and its posture information is detected. . And in the said image, the feature point same as the feature point extracted by the said pre-movement process is extracted, and the present relative position to the feature point is measured. At the same time, the current posture information is also detected. These pieces of information (characteristic point relative position information and posture information) are transmitted to the main processing unit 10 and stored in the main computer 45 (step S3, post-movement process).
[0051]
Subsequently, when the relative position information of the feature points at the respective time points before and after the time and the posture information of the moving body 1 itself are transmitted to the main computer 45, the computer 45 first determines the moving body from the posture information. Posture change information, which is its own posture change amount, is calculated (step S4). After that, by substituting this posture change information and each relative position information up to the feature point at each time point into a predetermined equation (Equation 9 to be described later), the moving body 1 itself moves back and forth in time. The self-position at a later time with respect to the position at the previous time is calculated (step S5, calculation step).
[0052]
Then, by repeating the above process, the self-position at each time point can be calculated, so that the moving path of the moving body 1 can be calculated. Specifically, when the next sampling time further elapses, the acquired image and the like at that time become those at the new later time, and at that time, the later time is the new earlier time. Since it is updated, the relative movement amount between the two points can be calculated.
[0053]
Here, expressions used in the calculation step of step S5 will be described. The stereo camera 2 may not be fixed as described in other embodiments, but in this embodiment, as described above, the case where the stereo camera 2 is fixed to the moving body 1 will be described. To do. The moving body 1 located in FIG. 1 (a) before moving is moved so as to be located in FIG. 1 (b), where t is the time before moving (destination) and t + 1 is the time after moving (after). The case will be described.
[0054]
In FIG.
X_B0: Body coordinate system at time t
X_C0: Camera coordinate system at time t
X_B1: Body coordinate system at time t + 1
X_C1: Camera coordinate system at time t + 1
^B0X_P: Body coordinates of point P at time t
^C0X_P: Camera coordinates of point P at time t
^B1X_P: Body coordinates of point P at time t + 1
^C1X_P: Camera coordinates of point P at time t + 1
φ: Change amount of posture angle (roll angle) before and after the moving body 1 moves
θ: Amount of change in posture angle (pitch angle) before and after the moving body 1 moves
ψ: change amount of posture angle (yaw angle) before and after movement of moving body 1
It is.
[0055]
X now_C0To X_B0Simultaneous conversion matrix to^B0 _C0S, if the camera is fixed with respect to the moving body 1, before and after the movement^B0 _C0S is immutable (ie^B1 _C1S =^B0 _C0S), the following equation is obtained.
[0056]
[Expression 1]

[0057]
[Expression 2]

[0058]
By the way, the above simultaneous conversion matrix^B0 _C0S is a matrix determined in advance because the stereo camera 2 is fixed.
[0059]
X_B0And X_B1Simultaneous conversion matrix between^B0 _B1S, the translation transformation matrix^B0 _B1T, rotation transformation matrix^B0 _B1R and X_B0And X_B1The amount of translation between t_x, T_y, T_zgiven that,
[0060]
[Equation 3]

[0061]
[Expression 4]

[0062]
[Equation 5]

[0063]
[Formula 6]

It becomes. here,
[0064]
[Expression 7]

given that,
[0065]
[Equation 8]

So,
[0066]
[Equation 9]

It can be expressed.
[0067]
Therefore, predetermined^B0 _C0It is the coordinates of the measured point P in the formula 9 including S^C0X_Pas well as^C1X_P, Furthermore, it is represented by the amount of change in posture angle^B0 _B1By substituting R, the amount of movement (t_x, T_y, T_z).
[0068]
Subsequently, the technique for measuring the position of the feature point shown in step S1 and step S3 of FIG. 3 will be described in detail with reference to FIGS. FIG. 4 is a flowchart showing an operation for measuring the position of a feature point. FIG. 5 to FIG. 6 are diagrams showing how images acquired by the stereo camera 2 are processed.
[0069]
First, an outline of feature point position measurement will be described. Basically, the procedure is to select a feature point and measure the position of the feature point (steps S13, 14, 15). However, in the position measurement of the feature point after movement, the feature point selected before movement is selected. Is tracked and the position is measured (steps S16, 14, 15). First, the extraction of feature points when the moving body 1 is in the position before the movement (destination) and the operation of the position measurement will be described.
[0070]
First, the moving body 1 acquires a surrounding image with the stereo camera 2 in accordance with an instruction from the main computer 45 or the image processing unit 41 (step S11, FIG. 5A). At this time, the master image acquired from one camera and the slave image acquired from the other camera are respectively stored in an odd field and an even field on one frame memory as shown in FIG. As a result, the NTSC image processing apparatus can be made compact in configuration.
[0071]
Here, in the present embodiment, the master image is deleted from the stereoscopically viewed image by compression to obtain a slave image (FIG. 5B), and this slave image is temporarily stored in the image processing unit 41. At the same time, the even and odd lines of the image are adjusted (FIG. 5C), the slave image is erased by compression, and the master image is acquired (FIG. 5D). Then, this master image is temporarily stored in the image processing unit 41 like the slave image, and a copy of the master image (FIG. 5 (e)) is used as a master image before movement (previous time). Separately stored in the processing unit 41 or the main computer 45. This is an image used when a feature point that should be tracked after movement is lost.
[0072]
Here, the acquired slave image (FIG. 5B) and master image (FIG. 5D) are transmitted to the main processing unit 10 and displayed on the monitor 10b. Thereby, the operator in a remote place can visually recognize the surrounding situation seen from the mobile body 1.
[0073]
Incidentally, an image such as a master image stored in the image processing unit 41 may be stored in the main computer 45. This depends on whether the feature point extraction processing is performed by the image processing unit 41 or the main computer 45, but there is no difference in the configuration regardless of which arithmetic means is used. Therefore, at this time, the master image (d), the slave image (b), the copy of the master image (e), and the contour extraction image (g) are stored in the image processing unit 41 or the main computer 45.
[0074]
Next, since it is a feature point extraction process before moving, and a feature point (tracking point) has not yet been selected, an extraction process that is a selection of a new feature point is performed (update of feature points). Requested (Yes in step S12)). For this reason, in the present embodiment, the master image (FIG. 5D) temporarily held is binarized (FIG. 5F), and the contour extraction process is performed (FIG. 5G). . Based on this image, the process shown in FIG. 6 is performed in order to select, as a feature point, a location that meets a predetermined criterion. However, the procedure for selecting feature points is not limited to the following.
[0075]
First, as shown in FIG. 6A, the contour extraction image is divided into nine regions. The reason why the screen is not included in the divided areas is to prevent the feature points from being detached from the image after the movement. Then, a rectangular template window W1 (for example, 20 × 20 pixels) is set in a certain area, and the template window is moved by several pixels from the upper left to the lower right as indicated by reference numerals W2 and W3, The grayscale feature amount is extracted (FIG. 6B). At this time, since the target image is a binary contour image, the feature amount is detected only when the template has a white contour line, and the coordinates of the upper left point of the template at that time are stored (for example, Point C for template window W2. The area to be processed may be at least one of the nine areas, and may be determined in advance so that only the upper left area is processed as shown in FIG. However, as for the processing region, as described in other embodiments, a plurality of regions at locations corresponding to the operation state of the moving body 1 may be processed.
[0076]
After the coordinates of all points for which feature amounts have been detected are stored in this way, the grayscale feature amounts in the template at each coordinate stored in the master image that is not a binary image are extracted (FIG. 6C, ( d)). In the present embodiment, the light / dark feature amount is the light / dark difference between the maximum density pixel and the minimum density pixel in the template W2. Then, the template having the maximum feature amount (light / dark difference) is used as a feature point, and the coordinates of the point are stored (detection of feature point, step S13). However, the method of calculating the feature points is an example, and is not limited to this method.
[0077]
Subsequently, a point (matching point) corresponding to the feature point extracted in the master image is searched on the slave image (step S14). For this reason, first, the grayscale feature amount of the feature point (template window W2 corresponding to the feature point) is extracted from the master grayscale image (FIG. 5D) and normalized on the slave grayscale image (FIG. 5B). Take correlation. As a result, when a point corresponding to the feature point is found on the slave image, the coordinates of the point are stored. Note that matching by normalized correlation is a well-known technique that has been used in the past, and thus detailed description thereof is omitted.
[0078]
After that, from the coordinates of the feature point (tracking point) detected on the master image and the point (matching point) corresponding to the feature point detected on the slave image, an arithmetic unit such as the image processing unit 41 or the main computer 45 is used. Thus, the three-dimensional coordinates of the feature points are measured based on the principle of stereo vision (step S15). The measured three-dimensional coordinates are the camera coordinates of the point P before the movement, that is, at time t in FIG.^C0X_PIs stored in the main computer 45. Here, since the measurement of the three-dimensional coordinates by stereo vision is a well-known technique, the description thereof is omitted.
[0079]
By the way, even if feature point extraction processing before moving is not performed, tracking of feature points extracted in the previous image fails, or if there is a request for feature point update due to some error, it is stored separately. A feature point extraction process is performed using the master image (FIG. 5 (e)) at the position of the tip (Yes in step S 12, step S 13).
[0080]
Then, after the moving body 1 moves, that is, when a predetermined time elapses from the previous image acquisition and the time becomes t + 1 (FIG. 1B), the stereo camera 2 acquires the image again. (Step S11), feature points are tracked. In such a case, since there is no feature point update request (No in step S12), detection is performed by matching the same feature point as the feature point extracted in the previous image from the newly acquired image ( Steps S16 and S14). Specifically, the light and dark feature values of the feature points extracted in the previous master light and shade image are extracted, and the normalized correlation is obtained on the current master light and shade image. If the corresponding point is detected, the coordinates of the point are stored (step S16). Further, the point corresponding to the feature point in the current master image is normalized on the slave image as described above, and when the corresponding point is detected, the coordinates of the point are stored (step S14). ).
[0081]
Thus, the camera coordinates of the point P viewed from the moving body 1 from the coordinates of the feature points on the master and slave images after the movement (time t = t + 1).^C1X_PAre measured based on the principle of stereo vision (step S15).
[0082]
Incidentally, as described above, at the time of image acquisition (step S11), the posture angle of the moving body 1 is detected both before the movement (time t = t) and after the movement (t = t + 1). Is called. Such information may be held in the CPU of the moving body itself or may be stored in the main computer 45. Then, after the post-movement posture angle information is detected, the amount of change (φ, ψ, θ) is calculated as compared to before the movement. Each value of these change amounts is expressed as a rotation transformation matrix as shown in the above equation (5).^B0 _B1Expressed as R.
[0083]
Measured in this way^C0X_P,^C1X_P,^B0 _B1Predetermined from R^B0 _C0The translational movement amount of the moving body 1 is calculated by an arithmetic means such as the main computer 45 using Equation 9 including S. Then, the position after the movement (t = t + 1) is set as the position before the new movement, and the position after the subsequent movement is set as the new position after the movement (t = t + 2). The amount can be measured. By connecting these, the moving path and the current position of the moving body 1 can be measured.
[0084]
Accordingly, since the self-position can be measured by tracking at least one feature point with the attitude angle of the mobile body 1 and the camera 2 mounted on the mobile body 1, the self-position can be stably measured. And the measurement accuracy can be improved.
[0085]
<Second Embodiment>
Next, a second embodiment of the present invention will be described with reference to FIG. FIG. 7 is a schematic view showing an operation state in the second embodiment of the present invention. In particular, FIG. 7A is a diagram illustrating a state at time t, and FIG. 7B is a diagram illustrating a state at time t + 1.
[0086]
In the following operations in this embodiment, each operation program is stored in advance in the main computer 45, the image processing unit 41 in the moving body 1, and the CPUs 42, 43, 44, as in the first embodiment. Realized by being incorporated.
[0087]
In this embodiment, it has substantially the same configuration as the moving body 1, that is, the self-position measuring device in the first embodiment described above, and the self-position is measured by substantially the same method. However, the present embodiment is different from the first embodiment in that the stereo camera 20 is not fixed to the moving body 1.
[0088]
As shown in FIG. 7B, the stereo camera 20 is not fixed to the moving body 1 and can be moved by an operator remotely operating from the main processing unit 10 shown in FIG. Then, there are cases where more useful visual information (image) can be obtained by changing the pan / tilt angle, height, and the like of the stereo camera 20 (reference numeral 20 ′ indicates when the stereo camera is moved). Show the situation). In such a case, the translational movement amount of the moving body 1 can be obtained by using the following calculation formula. Here, the movable amount with respect to the fixed position of the stereo camera, that is, the three-dimensional translational movement amount (a, b, c) when moving from the position indicated by reference numeral 20 in FIG. The rotational movement amount (roll angle α, pitch angle β, yaw angle γ) can be detected by providing the camera with a sensor such as a potentiometer or a rotary encoder.
[0089]
In FIG.
X_C1: Camera coordinate system at time t + 1 when the camera is fixed
X_{C1 '}: Camera coordinate system at time t + 1 when the camera is not fixed
^C1X_P: Camera coordinates of point P at time t + 1 when the camera is fixed
^{C1 '}X_P: Camera coordinates of point P at time t + 1 when the camera is not fixed. If the translational movement amounts of the camera coordinates with respect to the moving body body coordinates of the stereo camera before and after the movement are a, b, and c, and the rotational movement amounts are α, β, and γ, then X_{C1 '}To X_C1Simultaneous conversion matrix to^C1 _{C1 '}S is
[0090]
[Expression 10]

[0091]
## EQU11 ##

[0092]
[Expression 12]

[0093]
As described above, the simultaneous conversion matrix is obtained by using the translational movement amounts a, b, c and the rotational movement amounts α, β, γ of the camera coordinates relative to the moving body body coordinates of the stereo camera by the operation of the operator.^C1 _{C1 '}S can be calculated. Therefore, the translational movement amount of the moving body 1 is the same as when the stereo camera 20 is fixed,
[0094]
[Formula 13]

[0095]
It becomes. Therefore, first, the translational movement amounts (a, b, c) and the rotational movement amounts (α, β, γ) when the camera is moved are used.^C1 _{C1 '}S is calculated,^B0 _C0Substituting into equation (13) together with S, this equation is used as a predetermined equation. Then, in the same way as in the first embodiment, by substituting the position coordinates from the moving body 1 to the characteristic point P before and after the movement and the posture angle change amount of the moving body, the equation 13 The self-position of the body 1 can be measured.
[0096]
<Third Embodiment>
Hereinafter, a third embodiment of the present invention will be described with reference to FIG. In this embodiment, although it has the structure substantially the same as other embodiment mentioned above, the method of measuring a self position differs in the following points. Note that the following operations are realized by incorporating a program for performing calculations in order to implement the following method in calculation means such as the image processing unit 41 and the main computer 45.
[0097]
First, the moving body 1 selects a plurality of feature points based on a predetermined criterion in the pre-movement process, and measures the relative position of each feature point. For example, as described above, the acquired image is divided into nine regions, four regions are selected from among the nine regions, and feature points are extracted from each region. The selection of the four areas is performed according to the operation mode of the moving body 1, and an example thereof is shown in FIGS. In these drawings, each hatched area is a region from which feature points are extracted. FIG. 8A shows a case where the moving body moves forward, FIG. 8B shows a case where the moving body moves backward, and FIGS. d) is a case where the mobile body rotates clockwise and counterclockwise, respectively.
[0098]
As described above, when feature points are extracted from the master image, feature points are matched in the slave image, and the coordinates of the feature points are measured. This is done for each of the four points. Thereafter, after the moving body is moved, the four extracted feature points are tracked, and the respective coordinates are measured. At this time, the measurement method is the same as the method described above.
[0099]
Here, for example, when the moving body 1 moves forward, the four shaded areas in FIG. 8A are selected as the feature point extraction areas. However, if the moving direction of the moving body is the arrow Y1 direction, the feature points are selected. Since the moving direction of the arrow is the arrow Y2, the feature points are within the image, and tracking can be continued. Similarly, in each case, the processing area is such that the feature point is within the area even if the moving body moves.
[0100]
And the self-position after the movement with respect to before the movement of the mobile body 1 is calculated for each feature point by the same method as the method described above. At this time, some of the feature points cannot be tracked. In such a case, the movement amount is calculated by trusting that the tracking has continued for a longer period. Incidentally, when the feature point is lost in tracking, a new feature point update / extraction request is issued, and position measurement is performed for the extracted new feature point.
[0101]
In this way, it is possible to improve the accuracy of the measurement value by tracking a plurality of feature points, and to make the processing target area for the acquired image within the necessary range, and the processing burden of the apparatus Can be suppressed. In addition, by setting the processing region according to the operation mode of the moving body, the tracking of the feature points can be continued for a long time, and the position measurement accuracy of the moving body can be further improved.
[0102]
Here, the feature point extraction described above is not limited to four points. It may be one point as in the first embodiment described above, or another number of plural points. In addition, the measurement data at the reliable feature points among the four points is not limited to adopting the average value of a plurality of feature points, and the measurement value is calculated using the least square method or the like. May be.
[0103]
<Fourth Embodiment>
Next, a fourth embodiment of the present invention will be described. In this embodiment, since a plurality of feature points are extracted to calculate the self position of the mobile body itself, the configuration is almost the same as that of the third embodiment described above, but the following configuration is further adopted. ing. In the following description, FIGS. 1 to 8 are used as necessary.
[0104]
In addition, the operation described below can be realized by incorporating each operation program in advance in the main computer 45, the CPU in the moving body 1, or the like, as in the other embodiments described above. That is, functions for realizing the following operations are constructed in the main computer 45, the image processing unit in the moving body 1, the CPU, and the like, and each data related to position measurement is transmitted to the main computer 45 and the like, and the following processing is performed. Is done.
[0105]
In the present embodiment, first, as in the other embodiments described above, the position measuring means 2 is configured by a stereo camera including a plurality of cameras that can be viewed in stereo, and is shown in steps S1 and S3 of FIG. In the pre-movement process and post-movement process, each feature point corresponding to each image is detected by matching a plurality of acquired images from each camera using a stereo camera, and each feature point is detected based on the principle of stereo vision. Measure the relative position of. Since the specific method for measuring the relative position has been described above, a description thereof will be omitted.
[0106]
In the post-movement process shown in step S3 of FIG. 3, when matching the images acquired by the respective cameras, a value relating to the degree of coincidence by the matching is calculated and stored for each feature point in the calculation means. To do. Specifically, after a predetermined sampling time has elapsed (after movement), the moving body 1 acquires an image and performs tracking (tracking) of feature points. First, from a master image acquired by one camera, The feature point that is the same as the feature point selected in the pre-movement process is detected using the above-described shade difference as the shade feature amount (step S16 in FIG. 4). Thereafter, a point corresponding to the feature point on the master image is detected by taking a normalized correlation in the slave image and performing a matching process (step S14 in FIG. 4). At this time, the normalized correlation value represents the matching degree of the matched images and is set to take a value between 0 and 1. For example, it is set to 1 when the image patterns completely match. In the present embodiment, this value is used as a “weight”, and a value indicating a ratio of which movement amount is emphasized among the movement amounts that are respective self-positions obtained from a plurality of feature points. It becomes.
[0107]
Therefore, in the calculation step, a final self-position is calculated by adding a weight according to a value related to the degree of coincidence of each feature point to each measured value of the self-position measured for each of the plurality of feature points. At this time, the weight assigned to each measurement value is set so as to increase as the degree of coincidence by matching between the master image and the slave image at the time of detecting feature points increases.
[0108]
Specifically, when n feature points can be tracked, as described above, the moving amount x of the moving body 1 calculated from the relative position of each feature point using Equation 9 or Equation 13 as described above._{1 to n}(Here, only the amount of movement in the x direction is considered), the correlation value c_{1 to n}Are added as weights, and these weighted averages m_wIs calculated as shown in equation (14).
[0109]
[Expression 14]

[0110]
Then, for each feature point, a weighted average m of each movement amount (x, y, z)_wThis value is used as the final measurement value of the movement amount.
[0111]
Thereby, the final movement amount of the moving body 1 is calculated by largely reflecting the ratio of the movement amount calculated from the feature points having a high degree of matching at the time of matching by the stereo camera, that is, the feature points having a large correlation value. The Rukoto. Since a feature point with a large correlation value is likely to be a feature point on a stationary object, tracking of other moving objects is suppressed, and the amount of movement is suppressed. The current position of the body can be measured.
[0112]
<Fifth Embodiment>
Next, a fifth embodiment of the present invention will be described. In the present embodiment, similar to the fourth embodiment, a plurality of feature points are extracted, the movement amount of the device itself is calculated for each feature point, and the movement amount for each feature point is weighted to be finalized. The self-position is calculated. However, in this embodiment, the setting method of the weight when obtaining the weighted average shown in the above equation 14 is different, and the following configuration is adopted. In the following description, FIGS. 1 to 8 are used as necessary.
[0113]
The operations described below can be realized by incorporating each operation program in advance in the main computer 45, the image processing unit in the moving body 1, the CPU, and the like, as in the other embodiments described above. That is, functions for realizing the following operations are constructed in the main computer 45 and the CPU in the moving body 1.
[0114]
In the present embodiment, in the above-mentioned pre-movement process and post-movement process shown in steps S1 and S3 of FIG. 3, the density of the acquired image is obtained using the position measurement unit 2 configured by a stereo camera as the image acquisition unit. A plurality of feature points are selected and detected based on the values, and the relative positions of the feature points are measured.
[0115]
At this time, the pre-movement process, when selecting each feature point, information on the gray value of each feature point, specifically, the difference between the maximum gray value and the minimum gray value of a predetermined area on the master image. A point on the highest region is selected as the feature point as the feature value (step S14 in FIG. 4). In the present embodiment, however, the feature value calculated particularly in association with each of the selected feature points. Are stored in the main computer 45, the CPU in the moving body 1, or a predetermined storage unit provided in the moving body 1.
[0116]
Further, in the calculation step, a final self-position is calculated by assigning a weight according to the density feature amount of each feature point to each measured value of the self-position measured for each of the plurality of feature points. At this time, the weight is a value of 0 to 1, but the weight is set as a value closer to 1 as the shade feature amount is larger. Specifically, it is calculated by the formula shown in Formula 15. Note that w is a weight and works in the same way as c in Equation (14). That is, w is calculated by the number n of feature points._{1 to n}Is calculated. Moreover, g is a light / dark difference, and this is also set for each feature point.
[0117]
[Expression 15]

[0118]
Note that the weight is calculated as shown in the above equation. The density of the image is normally handled as 8-bit data, and the density difference takes a value of 0 to 255. Therefore, it is for converting this into a value of 0-1.
[0119]
The weight w (w_{1 to n}) Is obtained for each feature point, and a weighted average is calculated in the same manner as the above equation (14). As a result, a portion (edge) where the density of the image changes rapidly is selected as the feature point, and the reliability of the feature point is considered high. Such feature points are also considered to be feature points on a stationary object. Therefore, by calculating the final movement amount that largely reflects the ratio of the movement amount of the large shade feature amount, the influence of other moving objects is suppressed, and the current position of the moving body 1 is measured more accurately. can do.
[0120]
<Sixth Embodiment>
Next, a sixth embodiment of the present invention will be described. In this embodiment, as in the fourth to fifth embodiments, a plurality of feature points are extracted, the movement amount of the device itself is calculated for each feature point, and the movement amount for each feature point is weighted. The final self-position is calculated. However, in the present embodiment, the weight setting method for obtaining the weighted average that is the final movement amount of the moving body 1 in Equation 14 is different, and the following configuration is adopted. In the following description, FIGS. 1 to 8 are used as necessary.
[0121]
In addition, the operation described below can be realized by incorporating each operation program in advance in the main computer 45, the CPU in the moving body 1, or the like, as in the other embodiments described above. That is, functions for realizing the following operations are constructed in the main computer 45 and the CPU in the moving body 1.
[0122]
The post-movement process and the calculation process shown in step S1 and step S3 of FIG. 3 in the present embodiment operate repeatedly after the self-position is calculated by the calculation process, and at that time, in the post-movement process that has been operated immediately before The plurality of extracted feature points operate as a plurality of feature points selected in the pre-movement process. That is, as described in the first embodiment, while the moving body 1 is moving, the feature points are continuously tracked and the amount of movement is accumulated to calculate the route. Yes.
[0123]
In particular, in the post-movement process, when each feature point is extracted, the value of the number of times each feature point is continuously detected is associated with the feature point and the image in the main computer 45 or the moving body 1. The data is stored in a processing unit or a predetermined storage unit. Specifically, the number of times of tracking continuously for each feature point is integrated and stored for each feature point. At this time, for example, by storing together with the identification number of the feature point and the density feature amount of the feature point, the number of times of tracking with other feature points can be distinguished and stored. Then, the newly selected feature point has its tracking count initialized, is counted from once, and the stored value is updated each time the track king count increases. Of these tracking times, the largest value is l_maxSeparately stored in the main computer 45, the image processing unit, and in another storage unit in the moving body 1. The maximum number of times is compared with the tracking number of each feature point every time the tracking number of any feature point is updated, and is updated when the value is greater than the maximum number of times.
[0124]
The number of times of tracking thus measured is used when calculating the final movement amount. That is, in the calculation process, after the self-position measurement is performed a plurality of times, each measurement value of the self-position measured for each of the plurality of feature points is weighted according to the number of times of detection of each feature point, and finally Calculate self-position. At this time, the weight w is l for the number of tracking times and l for the maximum value._maxThen, it is expressed as shown in Equation 16, that is, the weight assigned to each measurement value is set to increase as the detection count value increases. Thus, the weight is set between 0 and 1, and the feature point counting the maximum number of times is_max= L, and its weight is 1. Note that w works in the same manner as c in Expression 14. That is, w is calculated by the number n of feature points._{1 to n}Is calculated. The value of l is also set for each feature point.
[0125]
[Expression 16]

[0126]
After that, as in the fifth embodiment described above, a weighted average is calculated using Equation 14 above using the obtained weight w. As a result, it is possible to suppress the influence of the movement amount obtained from unstable feature points that can be continuously tracked only a small number of times, even though they are not out of sight, while on the other hand, many Since the ratio of the amount of movement from the tracked feature point is greatly reflected, the current position of the moving body can be measured with high reliability and accuracy. In addition, since the unstable feature points as described above are considered to be feature points on other moving objects, the influence of such moving objects can be suppressed.
[0127]
<Seventh embodiment>
Next, a seventh embodiment of the present invention will be described. In the present embodiment, as in the fourth to sixth embodiments, a plurality of feature points are extracted, the movement amount of the device itself is calculated for each feature point, and the movement amount for each feature point is weighted. The final self-position is calculated. However, in the present embodiment, the weight setting method for obtaining the weighted average that is the final movement amount of the moving body 1 in Equation 14 is different, and the following configuration is adopted. In the following description, FIGS. 1 to 8 are used as necessary.
[0128]
The operations described below can be realized by incorporating each operation program in advance in the main computer 45, the image processing unit in the moving body 1, the CPU, and the like, as in the other embodiments described above. That is, functions for realizing the following operations are constructed in the main computer 45, the image processing unit in the moving body 1, the CPU, and the like.
[0129]
First, in the present embodiment, similar to the sixth embodiment, the post-movement process and the calculation process shown in Step S1 and Step S3 of FIG. 3 are repeatedly operated after the self-position is calculated by the calculation process. In this case, the plurality of feature points extracted in the post-movement process that was operated immediately before are operated as the plurality of feature points selected in the pre-movement process. That is, as described in the first embodiment, while the moving body 1 is moving, the feature points are continuously tracked and the amount of movement is accumulated to calculate the route. Yes.
[0130]
The moving amount of the moving body obtained from the plurality of feature points is weighted for each feature point, and the final moving amount is calculated by obtaining a weighted average as shown in Equation 14. In the present embodiment, a weight corresponding to the amount of deviation from the next self-position predicted from the travel path so far is used. This will be described in detail below.
[0131]
First, in the calculation step, values related to the final self-position calculated comprehensively based on self-positions from a plurality of feature points so far are stored in the main computer 45, etc. Can be referred to.
[0132]
  And a calculation process performs the operation | movement shown to the following processes, after performing self-position measurement several times. That is, a predicted value calculation step for calculating the predicted self-position by predicting the self-position of the device itself based on the values related to the current self-position, and each measured value of the self-position measured for each of the plurality of feature points and the predicted self A comparison step for comparing each position and calculating the amount of deviation, and a final value by assigning a weight corresponding to the amount of deviation for each feature point to each measured value of the self-position measured for each feature point A final self-position calculating step of calculating the self-position. As will be described below, the deviation amount is small.BecomeAs a result, the weight assigned to each measurement value is set larger.
[0133]
The predicted value calculation step predicts the amount of movement at the next moment from the time-series data of the amount of self movement obtained at a constant sampling interval. This is because the movement of the robot, which is a moving body, is based on a certain physical law (equation of motion), and an extremely flying value is not measured. Specifically, the predicted movement amount is calculated using a Kalman filter.
[0134]
Here, the Kalman filter is a model of how the change occurs when time series data of the change amount can be measured in a certain system, and data on how much disturbance the measured value of the change amount and the system receives. This is a technique for accurately predicting, filtering, and smoothing the amount of change by giving it in advance. However, the details of the Kalman filter are omitted, and the calculation method of the predicted value will be described below.
[0135]
In order to apply the Kalman filter, first, the system is represented by the following basic system.
[0136]
[Expression 17]

[0137]
[Formula 18]

[0138]
Now, since the predicted value to be obtained is the moving amount of the moving body, this is set as (x, y, z), and the velocity and acceleration are respectively (v_x, V_y, V_z), (A_x, A_y, A_z), Equation 19 is obtained.
[0139]
[Equation 19]

[0140]
The same applies to y and z. At this time, it is defined as follows.
[0141]
[Expression 20]

[0142]
[Expression 21]

[0143]
[Expression 22]

[0144]
[Expression 23]

[0145]
If the above equation is used, the following equation can be obtained from the Kalman filter algorithm.
[0146]
[Expression 24]

[0147]
It becomes. K_kIs Kalman gain, Σ_x0Is x₀Is the covariance matrix. Here, since the right side of the expression at the bottom of Expression 24 is generally unknown, the expression cannot be determined. Therefore, the following equation (25) is adopted as the initial value of the uppermost equation of equation (24) and the fourth equation from the top.
[0148]
[Expression 25]

However, O₆Is a 6-dimensional zero vector, I_6x6Represents a 6 × 6 unit matrix.
[0149]
[Equation 26]

[0150]
Also, the reason for defining the above equation is that the indeterminacy of the observation noise dispersion is collectively included in the initial value indefiniteness of the right equation of the above equation. Where the noise variance ratio σ_w ²/ Σ_v ²Finally remains in the Kalman filter as unknown parameters.
[0151]
From the above, dispersion ratio σ of system noise and observation noise_w ², Σ_v ²Is determined, the second equation from the top of Equation 24 is sequentially obtained as the state prediction vector including the predicted value of the movement amount. In the embodiment described below, the dispersion ratio is determined by trial and error.
[0152]
As described above, since the predicted value of the movement amount calculated by the Kalman filter is calculated based on the time series data of the past movement amount, it is calculated if there is no moving object in the field of view at the start of measurement. The predicted value is not affected by the moving object. Since the movement amount calculated from the feature point on another moving object is considered to cause a large deviation from the predicted value, the larger the deviation amount from the prediction value with respect to the movement amount for each of the obtained feature points, By applying a filter that applies a small load, and on the other hand, a larger load is applied as the deviation amount is smaller, the influence of other moving objects can be suppressed. Thereby, the movement amount with a higher system can be calculated.
[0153]
Here, in the present embodiment, statistical theory is applied, and the deviation from the predicted value is considered to follow a normal distribution having the predicted value as an average, and the variance σ of the normal distribution based on the actually measured value is considered.²And the value of this normal distribution function is used as a weight. That is, the weight is set to 1 when the movement amount measured from each feature point is the same as the predicted value, and the weight is set to approach 0 as the distance from the predicted value increases. As will be described below, the normal distribution function is appropriately set by trial and error, and the distribution is stored and held in the main computer 45, the CPU in the moving body 1, or a predetermined storage unit.
[0154]
(Example)
Here, as described above, an experiment is described in which a predicted value of the moving amount of the moving body 1 is calculated using the Kalman filter, and the self-position is measured from a plurality of feature points using a weight based on the predicted value. To do. In this experiment, the moving body 1 on which the self-position measurement system shown in FIGS. 1 and 2 is mounted is moved forward, and another moving object 100 is moved within the camera field of view of the moving body 1. The moving amount of the moving body 1 is measured. Then, the effectiveness of the Kalman filter is verified.
[0155]
Here, in such an experiment, it is also necessary to measure the actual moving amount (dead reckoning) to be compared with the moving amount measured by the present system of the moving body 1. For this, the rotational speed and the steer angle of the wheel are measured by a rotary encoder, and the position is estimated by odometry, which is a method of measuring the position by integrating this, and the measured value is handled as a theoretical value. In the experimental environment, the floor on which the moving body 1 travels is a flat surface, and slippage is unlikely to occur, so the measurement of the straight travel distance is within an error rate of 1%.
[0156]
Further, another moving object 100 used in the experiment moves differently from the moving body 1 that measures its own position, and an outline of the configuration is shown in FIG. As shown in this figure, another moving object 100 includes a self-running wheel 102 driven by a drive source (not shown) in a chassis 101, and a black flock ball that can be a feature point that the moving body 1 can easily recognize. 103 is provided at four higher positions.
[0157]
Further, the dispersion ratio σ of the system noise and the observation noise shown in the fourth formula from the above formula 24_w ², Σ_v ², And variance σ of deviation between measured value and predicted value²Need to be determined. Where σ_v ²Can be measured in advance, while σ_w ²Was determined by trial and error. Also, σ²Is σ_w ², Σ_v ²Based on the value measured after deciding, trial and error were performed for adjustment. These values are stored in the main computer 45, the CPU in the moving body 1, a storage unit, and the like so that they can be substituted into each mathematical expression or referred to when measuring the self position.
[0158]
FIGS. 10 to 13 show how the experiment was actually performed in this manner. Here, in the example shown in FIG. 10A, the moving body 1 that is the self-position measuring device itself is stationary toward the wall 200, and the other moving object 100 is lined as shown by an arrow A100. This is a case of moving along L100. At this time, an object (not shown) that can be various feature points exists in front of the moving body 1, and a figure that becomes a feature point is drawn on the wall 200.
[0159]
FIG. 10B shows a measurement result of the self position of the moving body 1. The horizontal axis represents the measurement time t [s (seconds)], and the vertical axis represents the movement distance tx [mm] of the moving body 1 in the wall direction 200. As shown in this figure, the measured value of the stationary moving body 1 is almost zero. That is, the self-position can be measured with high accuracy without being substantially affected by the other moving object 100 when stationary. In this experiment, only the distance x in the direction of the wall 200 is measured as the amount of movement, but the present invention is not limited to this. Of course, as described above, the amount of movement in three dimensions (x, y, z) can also be measured.
[0160]
Next, as shown in FIG. 11A, a case where another moving object 100 is moved in the direction of the arrow A100 along the line L100 and the moving body 1 is advanced in the direction of the arrow A1 will be described. Specifically, the measurement was performed in an environment in which the moving body 1 that measures its own position is overtaken by another moving object 100. And the measurement result of the self-position at that time is shown in FIG.11 (b). In this figure, similarly to FIG. 10B, the horizontal axis represents the measurement time t [s (seconds)], and the vertical axis represents the movement distance tx [mm] of the moving body 1, and dead reckoning. Shows the theoretical value (solid line) of the amount of movement and the measured value (diamond plot) of the amount of movement by this system. Then, it can be seen that the measured value is almost the same as the theoretical value, and the self-position can be measured with high accuracy.
[0161]
Furthermore, as shown in FIG. 12A, a case where another moving object 100 is moved in the arrow A101 direction along the line L101 and the moving body 1 is advanced in the arrow A1 direction will be described. Specifically, the measurement was performed in an environment in which the moving body 1 that measures its own position passes by another moving object 100. And the measurement result of the self-position at that time is shown in FIG.12 (b). And in this figure, the theoretical value (solid line) of the movement amount by dead reckoning and the measurement value (diamond plot) of the movement amount by this system are shown, but even in such an experimental example, the measurement value is It can be seen that there is almost no difference from the theoretical value, and it is possible to measure the self-position with high accuracy.
[0162]
As described above, by measuring the self-position using the filter based on the predicted value, even if another moving object 100 exists in the field of view of the camera, the other moving object 100 Highly accurate self-position measurement can be realized by suppressing the influence of existence. That is, even when another moving object 100 is followed as a feature point, the influence can be reduced and the self-position can be measured.
[0163]
<Eighth Embodiment>
Next, an eighth embodiment of the present invention will be described. In the present embodiment, an acceleration sensor is provided in the moving body that is the self-position measuring device having the above-described configuration. Then, the main computer 45 or the like which is a calculation means measures the acceleration of the moving body itself, measures the acceleration of other objects in the photographed image, and compares these to compare other moving objects. It has a function of detecting an object and removing the other moving object from the feature point. For example, when the difference from the acceleration of the moving body 1 is larger than a predetermined allowable value, a predetermined area on the moving object in the image is excluded from the feature point extraction area.
[0164]
As a result, it is possible to improve the accuracy of position measurement by suppressing other moving objects in the acquired image from becoming feature points, and the present invention is also in places where such moving objects exist. The self-position can be measured by the moving body, and the versatility can be improved.
[0165]
<Ninth embodiment>
Next, a ninth embodiment of the present invention will be described. In the ninth embodiment, the self-position measuring device is a portable type that a human being always wears. For example, it is a device that can be worn on a climber's hat, and is equipped with a stereo camera, a tilt angle sensor, a CPU, and the like. Data acquired by a stereo camera or the like may be transmitted to a remote computer via wireless communication means and processed by the computer.
[0166]
By doing so, it becomes easy to measure the moving route of the moving human or to grasp the current position by another person. And this measured value can be utilized for rescue when a climber suffers, for example.
[0167]
【The invention's effect】
Since the present invention is configured and functions as described above, according to this, the relative position coordinates of at least one of the surrounding feature points before and after the movement and the change information of the posture angle of the self are measured, and these are determined in advance. By calculating by substituting into the given formula, it is possible to measure the self-position after movement with respect to the position before movement, so that it is possible to perform stable and highly accurate movement path measurement and position measurement. Has no excellent effect. In particular, the effect is remarkable in terms of stability with respect to the conventional method of measuring the self-position from at least four feature points.
[0168]
In addition, it is possible to further improve the accuracy of measured values by extracting multiple feature points, measuring the self-position of each feature point, and determining the self-position comprehensively from those statistics. Can do.
[0169]
In addition, each of the self-positions obtained from a plurality of feature points is weighted to determine the final self-position, and a large weight is assigned to the self-positions from feature points considered to be highly reliable. Thus, even when other moving objects exist in the vicinity, the influence of such objects can be suppressed, and more accurate self-position measurement can be realized.
[Brief description of the drawings]
FIG. 1 is a schematic diagram showing an operation state in a first embodiment of the present invention. FIG. 1A is a diagram illustrating a state at time t, and FIG. 1B is a diagram illustrating a state at time t + 1.
FIG. 2 is a block diagram showing a configuration in the first embodiment of the present invention.
FIG. 3 is a flowchart showing an outline of a procedure for measuring a self-position in the first embodiment of the present invention.
FIG. 4 is a flowchart illustrating an operation during image processing according to the first embodiment.
FIGS. 5A to 5H are explanatory diagrams illustrating image processing states in the first embodiment. FIG.
FIGS. 6A to 6D are explanatory diagrams illustrating processing states when extracting feature points in the first embodiment. FIGS.
FIG. 7 is a schematic view showing an operation state in the second embodiment of the present invention. FIG. 2A is a diagram illustrating a state at time t, and FIG. 2B is a diagram illustrating a state at time t + 1.
FIGS. 8A to 8D are explanatory diagrams showing specific examples of image processing areas in the third embodiment of the present invention. FIGS.
FIG. 9 is a schematic view showing another moving object used in the tenth embodiment of the present invention.
FIG. 10 is a diagram showing an example of a measurement experiment in the tenth embodiment of the present invention. FIG. 10A is an explanatory diagram showing the state of the experiment, and FIG. 10B is a diagram showing the measurement result.
FIG. 11 is a diagram showing an example of a measurement experiment in the eleventh embodiment of the present invention. Fig.11 (a) is explanatory drawing which shows the mode of experiment, FIG.11 (b) is a figure which shows a measurement result.
FIG. 12 is a diagram showing an example of a measurement experiment in the twelfth embodiment of the present invention. FIG. 12A is an explanatory diagram showing the state of the experiment, and FIG. 12B is a diagram showing the measurement result.
[Explanation of symbols]
1 Mobile object
2 Stereo camera (position measuring means)
3 Attitude angle sensor (Attitude detection means)
10 Main processing section
41 Image processing unit (position measuring means, computing means)
42 CPU (calculation means)
45 Main computer (calculation means)

Claims (5)

A method of measuring the self-position after movement relative to the movement of the device itself,
Prior to the movement, a plurality of surrounding feature points are selected based on a predetermined criterion by a position measuring means provided in the device itself, and the relative positions of the respective feature points are measured, and the device itself is prepared. A pre-movement process for detecting posture information of the device itself by the posture detection means provided;
After the movement , the same feature point as the plurality of feature points selected in the pre-movement process is extracted, the relative position of each feature point is measured by the position measurement unit, and the device itself is detected by the posture detection unit. Post-movement process to detect posture information of
Using the calculation means provided in the device itself, the posture change information which is the posture change amount before and after the movement of the device itself is calculated from the detected posture information, and the device itself is moved for each of the plurality of feature points. by measuring its own position after the movement, and a calculation step of calculating the final self-position based on the plurality of measured values and said posture change information,
The calculation step stores the calculated value regarding the final self-position before and after the movement of the device itself in the calculation means,
The post-movement step and the calculation step operate repeatedly after the self-position before and after the movement of the device itself is calculated by the calculation step. At that time, the post-movement step extracted immediately before is extracted. Acts by considering a plurality of feature points as a plurality of feature points selected in the pre-movement process,
After the calculation step performs self-position measurement multiple times,
A predicted value calculation step of calculating a predicted self-position by predicting a self-position after movement of the device itself based on a value related to a self-position so far;
A comparison step of comparing each measured value of the self-position measured for each of the plurality of feature points with the predicted self-position, and calculating a deviation amount for each of the feature points;
A final self-position calculation step of calculating a final self-position by applying a weight according to the deviation amount calculated for each feature point to each measured value of the self-position measured for each of the plurality of feature points;
A self-position measuring method characterized by comprising: The arithmetic process, wherein as the shift amount becomes smaller set so that the weight to be given to each measurement value increases, the self-position measuring method of claim 1, wherein a. 3. The self-position measuring method according to claim 1, wherein the predicted value calculating step predicts a self-position after the movement of the device itself using a Kalman filter. A device for measuring the self-position after movement relative to the movement of the device itself,
Position measuring means for measuring relative positions of a plurality of feature points around the apparatus, attitude detecting means for detecting attitude information of the apparatus itself, and arithmetic means for calculating information obtained by controlling these operations. ,
The position measuring means measures the relative position of each feature point before and after the movement of the device, and the posture detection means acquires each posture information before and after the movement of the device itself,
The calculation means calculates posture change information, which is a change amount of the acquired posture information before and after the movement, and based on the posture change information and each relative position information of the feature point before and after the movement of the device itself. The function to calculate the self-position after movement relative to the movement of the device itself, and after measuring the self-position several times, the self-position after movement of the device itself is predicted based on the values related to the self-position so far A predicted self-position is calculated, each measured value of the self-position measured for each of the plurality of feature points is compared with the predicted self-position, and a deviation amount is calculated for each of the feature points, and the plurality of features A function of calculating a final self-position by attaching a weight according to the amount of deviation calculated for each feature point to each measured value of the self-position measured for each point .
Self-position measuring device characterized by that. Comprising an image acquisition unit for acquiring surroundings image of the device itself, a posture detection means for detecting a posture information of the device itself, and an arithmetic means for processing the image and the detected posture information, moving against the previous movement of the device itself A device that measures the self-position later,
A feature point position measurement function for measuring the relative positions of a plurality of feature points in a surrounding image using the image acquired by the image acquisition unit before and after the movement of the apparatus itself; and the posture detection unit. The posture change information calculation function for calculating the amount of change from the posture information before and after the movement of the device itself detected in Step 1, the position information of the feature points before and after the movement of the device itself, and the amount of change in the posture information are determined in advance. The movement amount calculation function for calculating the movement amount of the device itself by substituting it into the given equation and the self-position after the movement of the device itself based on the values related to the self-position after measuring the self-position several times Predicting the position to calculate the predicted self-position, comparing each measured value of the self-position measured for each of the plurality of feature points with the predicted self-position, and calculating the deviation amount for each of the feature points ,in front Each measured value of the self-position measured for each of the plurality of feature points, have a function of calculating the final self-position with a weight according to the deviation amount calculated above for each feature point,
Accordingly, a self-position measuring apparatus that measures the self-position after movement relative to the movement of the apparatus itself.

Images