JP2017207942A - 画像処理装置、自己位置推定方法、及び、プログラム - Google Patents

Abstract

【課題】自己位置の推定精度が低い。【解決手段】画像処理装置は、撮影された被写体の画像から抽出された前記被写体の特徴点を追跡する特徴点追跡部と、前記画像内の予め設けられたマークを認識するマーク認識部と、前記特徴点の追跡結果に基づいて第１自己位置を推定し、前記マークに基づいて第２自己位置を推定し、予め定められた条件に基づいて、前記第１自己位置及び前記第２自己位置のいずれか一方を自己位置として採用する自己位置推定部と、を備える。【選択図】図２

Description

本発明は、画像処理装置、自己位置推定方法、及び、プログラムに関する。

自動車及びロボット等の移動体に自立運転させるための技術として、未知の環境下で自立的に地図構築及び自己位置の推定を行う技術（例えば、ＳＬＡＭ：Simultaneous Localization And Mapping）が知られている。

上述の技術では、カメラで撮影された時間の異なる時系列の複数の画像等から複数の特徴点を抽出し、当該複数の特徴点を追跡したベクトルによって（例えば、オプティカルフロー）、自己位置を推定する。しかしながら、時系列で特徴点を追跡すると、追跡誤差が蓄積されて、自己位置の推定誤差が大きくなる。このような場合、ＲＦＩＤ及びランドマークによって自己位置を推定する技術が知られている。

しかしながら、上述の技術では、カメラがランドマークに対して相対移動している場合、ランドマークを撮像した画像によって自己位置を推定すると、モーションブラー等の影響によって誤差が生じ、自己位置の推定精度が低いといった課題がある。

本発明は、上記に鑑みてなされたものであって、自己位置の推定精度を向上させることを目的とする。

上述した課題を解決し、目的を達成するために、本発明は、画像処理装置は、撮影された被写体の画像から抽出された前記被写体の特徴点を追跡する特徴点追跡部と、前記画像内の予め設けられたマークを認識するマーク認識部と、前記特徴点の追跡結果に基づいて第１自己位置を推定し、前記マークに基づいて第２自己位置を推定し、予め定められた条件に基づいて、前記第１自己位置及び前記第２自己位置のいずれか一方を自己位置として採用する自己位置推定部と、を備える。

本発明は、自己位置の推定精度を向上できる。

図１は、実施形態の画像処理装置を有する画像処理システムの全体構成図である。 図２は、画像処理装置の機能を示すブロック図である。 図３は、フレーム画像である画像の一例を示す図である。 図４は、（ｎ−１）番目のフレーム画像である画像の一例を示す図の一部である。 図５は、ｎ番目のフレーム画像である画像の一例を示す図の一部である。 図６は、（ｎ−１）番目の画像とｎ番目の画像との間での特徴点の追跡を説明する図である。 図７は、マークの認識で扱う座標系とカメラの位置推定について説明する図である。 図８は、ピンホールカメラによる座標系とカメラの位置推定について説明する図である。 図９は、マークによるカメラの位置の推定における左右方向（または水平方向）の誤差を説明する図である。 図１０は、マークによるカメラの位置の推定における上下方向（または鉛直方向）の誤差を説明する図である。 図１１は、画像処理装置による自己位置推定処理のフローチャートである。 図１２は、別の自己位置の推定を説明する図である。 図１３は、飛行可能な移動体の自己位置の推定を説明するための上方から見た平面図である。 図１４は、飛行可能な移動体の自己位置の推定を説明するための側方から見た図である。 図１５は、本実施形態に係る画像処理装置のハードウェア構成を模式的に示すブロック図である。

以下の例示的な実施形態や変形例には、同様の構成要素が含まれている。よって、以下では、同様の構成要素には共通の符号が付されるとともに、重複する説明が部分的に省略される。実施形態や変形例に含まれる部分は、他の実施形態や変形例の対応する部分と置き換えて構成されることができる。また、実施形態や変形例に含まれる部分の構成や位置等は、特に言及しない限りは、他の実施形態や変形例と同様である。

＜実施形態＞
図１は、実施形態の画像処理装置１６を有する画像処理システム１０の全体構成図である。図１に示すように、画像処理システム１０は、マークＭＫと、カメラ１４と、画像処理装置１６とを備える。

マークＭＫは、ランドマークとも呼ばれ、駐車場等の壁または柱等の建造物９０に予め設けられた図形等である。マークＭＫは、カメラ１４によって撮影可能な位置に設けられている。マークＭＫは、画像処理装置１６によって認識が容易な標識、文字及び数字等であってもよい。

カメラ１４は、例えば、自動車等の移動体９２に設けられている。移動体９２は、飛行機、オートバイク、掃除機等の電気製品、農機具等であってもよい。カメラ１４は、矢印で示す移動体９２の内部の前方、例えば、バックミラー等に設けられている。カメラ１４は、移動体９２の前方の被写体を撮影して、画像（例えば、フレーム画像）を生成する。カメラ１４は、移動体９２の後方に設けられていてもよい。この場合、カメラ１４は、移動体９２の後方の被写体を撮影して画像を生成する。

画像処理装置１６は、カメラ１４とともに移動体９２に設けられていてもよく、移動体９２と異なる場所に設けられていてもよい。画像処理装置１６は、カメラ１４と情報を送受信可能に接続されている。画像処理装置１６は、カメラ１４から画像を受信する。画像処理装置１６は、受信した画像から建造物９０等の特徴点ＰＴを抽出するとともに、マークＭＫを認識して、自己の位置である自己位置ＳＰを推定する。特徴点ＰＴは、例えば、画像内に含まれる建造物９０等の被写体の角等のように際立って認識される点である。

図２は、画像処理装置１６の機能を示すブロック図である。図２に示すように、画像処理装置１６は、画像取得部２０と、フレームメモリ２２と、画像補正部２４と、画像メモリ２６と、特徴点抽出部２８と、特徴点格納メモリ３０と、特徴点追跡部３２と、追跡結果格納メモリ３４と、マーク認識部３６と、マーク情報格納メモリ３８と、マーク認識結果格納メモリ４０と、自己位置推定部４２とを備える。画像取得部２０、フレームメモリ２２、画像補正部２４、画像メモリ２６、特徴点抽出部２８、特徴点格納メモリ３０、特徴点追跡部３２、追跡結果格納メモリ３４、マーク認識部３６、マーク情報格納メモリ３８、マーク認識結果格納メモリ４０、自己位置推定部４２は、ソフトウェアで構成してもよく、ハードウェアで構成してもよく、一部をソフトウェアで構成して残りをハードウェアで構成してもよい。

画像取得部２０は、カメラ１４と接続されている。画像取得部２０は、カメラ１４によって撮影されたフレーム画像等の画像ＩＧを取得する。画像取得部２０は、画像ＩＧをフレームメモリ２２に格納する。画像取得部２０は、フレームメモリ２２に格納した画像ＩＧに関する情報（例えば、格納した画像ＩＧのライン数）等を画像補正部２４へ出力する。

画像補正部２４は、画像取得部２０から取得した画像ＩＧに関するライン数等の情報に基づいて、フレームメモリ２２から画像ＩＧを読み出す。尚、画像補正部２４は、画像ＩＧの全てではなく、一部をフレームメモリ２２から読み出してもよい。例えば、画像補正部２４は、予め定められたライン数の画像ＩＧがフレームメモリ２２に格納されると、当該画像ＩＧを読み出してもよい。画像補正部２４は、読み出した画像ＩＧに歪み補正等の補正処理を実行する。画像補正部２４は、補正した画像ＩＧを画像メモリ２６に格納する。

特徴点抽出部２８は、画像メモリ２６から補正された画像ＩＧを読み出す。尚、特徴点抽出部２８は、画像ＩＧの全てではなく、画像ＩＧの一部を画像メモリ２６から読み出してもよい。例えば、特徴点抽出部２８は、画像ＩＧのうち、予め定められたライン数の画像ＩＧの一部が画像メモリ２６に格納されると、当該格納された一部の画像ＩＧを読み出してもよい。特徴点抽出部２８は、読み出した建造物９０等の被写体の画像ＩＧから一または複数の特徴点ＰＴを抽出する。特徴点抽出部２８は、例えば、既知のハリス（Harris）またはモラベック（Moravec）等のコーナー検出法によって画像ＩＧから特徴点ＰＴを抽出する（例えば、http://ja.wikipedia.org/wiki/%E3%82%B3%E3%83%BC%E3%83%8A%E3%83%BC%E6%A4%9C%E5%87%BA%E6%B3%95参照）。特徴点抽出部２８は、抽出した特徴点ＰＴを特徴点格納メモリ３０に格納する。

特徴点追跡部３２は、撮影された建造物９０等の被写体の画像ＩＧから特徴点抽出部２８によって抽出された被写体の特徴点ＰＴを追跡する。例えば、特徴点追跡部３２は、時間的に１つ前の画像ＩＧから抽出された特徴点ＰＴを追跡結果格納メモリ３４から読み出す。特徴点追跡部３２は、１つ前の画像ＩＧに含まれる予め定められた必要画素の範囲の画像ＩＧを画像メモリ２６から取得する。特徴点追跡部３２は、特徴点ＰＴ及び１つ前の画像ＩＧに基づいて、後述するテンプレートパッチＴＰを作成する。特徴点追跡部３２は、現在の画像ＩＧを画像メモリ２６から読み出す。特徴点追跡部３２は、テンプレートパッチＴＰと、現在の画像ＩＧとに基づいて、ブロックマッチングにより特徴点ＰＴを検索して追跡する。特徴点追跡部３２は、特徴点ＰＴを追跡できた場合、追跡できた旨の結果であるオプティカルフローＦＬを含む追跡結果を追跡結果格納メモリ３４に格納して更新する。一方、特徴点追跡部３２は、特徴点ＰＴを追跡できなかった場合、特徴点格納メモリ３０から読み出した特徴点ＰＴを追跡結果格納メモリ３４に格納する。

マーク認識部３６は、画像ＩＧ内の建造物９０等の被写体に予め設けられたマークＭＫを認識する。例えば、マーク認識部３６は、特徴点格納メモリ３０に特徴点ＰＴが格納されると、マーク情報格納メモリ３８からマークＭＫに予め設定されたパターン情報及び位置情報を読み出すとともに、特徴点格納メモリ３０から特徴点ＰＴを取得する。マーク認識部３６は、特徴点ＰＴ及びパターン情報に基づいて、画像ＩＧ内のマークＭＫを認識する。マーク認識部３６は、マークＭＫの認識結果及び位置情報をマーク認識結果格納メモリ４０に格納する。尚、マーク認識部３６はマークＭＫを認識する処理を、特徴点追跡部３２が特徴点ＰＴを追跡する処理と同時に実行してもよい。

自己位置推定部４２は、追跡結果格納メモリ３４から追跡結果を読み出す。自己位置推定部４２は、追跡結果に基づいて第１自己位置を推定する。自己位置推定部４２は、マーク認識結果格納メモリ４０からマークＭＫの認識結果及び位置情報を読み出す。自己位置推定部４２は、認識されたマークＭＫに基づいて第２自己位置を推定する。例えば、自己位置推定部４２は、予め設定されたマークＭＫの位置情報に基づいて、第２自己位置を推定する。自己位置推定部４２は、予め定められた条件に基づいて、推定した２つの第１自己位置及び第２自己位置のいずれか一方を自己位置ＳＰとして採用する。

図３は、フレーム画像である画像ＩＧ０の一例を示す図である。図４は、（ｎ−１）番目のフレーム画像である画像ＩＧ１の一部の一例を示す図である。図５は、ｎ番目のフレーム画像である画像ＩＧ２の一部の一例を示す図である。図６は、（ｎ−１）番目の画像ＩＧ１とｎ番目の画像ＩＧ２との間での特徴点ＰＴの追跡を説明する図である。図３から図６は、移動体９２が道路９４上を直進している状態における画像ＩＧの例である。

画像取得部２０が図３に示すｎ番目の画像ＩＧ０をカメラ１４から取得して、画像補正部２４が補正した画像ＩＧ０を画像メモリ２６に格納した場合について説明する。この場合、例えば、特徴点抽出部２８は、図４に示す（ｎ−１）番目の画像ＩＧ１から特徴点ＰＴ１、ＰＴ２を抽出して、特徴点格納メモリ３０に格納する。特徴点ＰＴ１、ＰＴ２は、例えば、道路９４の傍の建造物９０Ａ、９０Ｂのコーナー（または、角）である。

特徴点追跡部３２は、（ｎ−１）番目の特徴点ＰＴ１、ＰＴ２を追跡結果格納メモリ３４から取得する。特徴点追跡部３２は、（ｎ−１）番目の画像ＩＧ０のうち、（ｎ−１）番目の特徴点ＰＴ１、ＰＴ２の周辺の数ラインの範囲の図４に示す画像ＩＧ１を画像メモリ２６から読み出す。特徴点追跡部３２は、（ｎ−１）番目の特徴点ＰＴ１、ＰＴ２を中心として、左右及び上下に数画素の範囲の矩形状のテンプレートパッチＴＰ１、ＴＰ２を（ｎ−１）番目の画像ＩＧ１から抽出する。テンプレートパッチＴＰ１、ＴＰ２は、円形状であってもよい。

特徴点追跡部３２は、図５に示すｎ番目の画像ＩＧ２を画像メモリ２６から取得する。特徴点追跡部３２は、類似度を用いたブロックマッチングによって、テンプレートパッチＴＰ１、ＴＰ２に類似する領域をｎ番目の画像ＩＧ２から検索する。例えば、特徴点追跡部３２は、ブロックマッチングとして、パターンマッチング(例えば、正規化相関)またはテンプレートマッチング（例えば、http://imagingsolution.blog107.fc2.com/blog-entry-186.html参照）等を実行する。特徴点追跡部３２は、画像ＩＧ２のうち、特徴点ＰＴ１と同じ座標の位置ＰＴ１’を含む領域ＡＲ１の内部でブロックマッチングによる検索を実行してもよい。特徴点追跡部３２は、領域ＡＲ１内において、左上端から右方向に検索して、領域ＡＲ１の右端に達した後は１ライン下の左端に移動して、再度右方向に検索する。特徴点追跡部３２は、この順序で画像ＩＧ２の領域ＡＲ１内の検索を順次続ける（いわゆる、ラスタスキャン）。特徴点追跡部３２は、（ｎ−１）番目のテンプレートパッチＴＰ１、ＴＰ２に類似する領域を、テンプレートパッチＴＰ３、ＴＰ４としてｎ番目の画像ＩＧ２から抽出する。特徴点追跡部３２は、テンプレートパッチＴＰ３、ＴＰ４の中心をｎ番目の特徴点ＰＴ３、ＰＴ４とする。図６に示すように、特徴点追跡部３２は、（ｎ−１）番目における特徴点ＰＴ１、ＰＴ２からｎ番目の特徴点ＰＴ３、ＰＴ４へと延びるベクトルであるオプティカルフローＦＬ１、ＦＬ２を追跡結果として、ｎ番目の特徴点ＰＴ３、ＰＴ４とともに、追跡結果格納メモリ３４に格納する。

図７は、マークＭＫの認識で扱う座標系とカメラ１４の位置推定について説明する図である。自己位置推定部４２は、三次元のマーク座標系（図７に示すＸｍＹｍＺｍ空間）、二次元の画像座標系（図７に示すｕｖ空間）、及び、三次元のカメラ座標系（図７に示すＸｃＹｃＺｃ空間）に基づいて、マークＭＫを認識する。マークＭＫの位置は、マークＭＫごとに予めマーク情報格納メモリ３８に登録されている。カメラ１４の内部パラメータ（カメラ１４の焦点距離、主点（画像中心）及びレンズ歪み係数等）は、既知であり、予め登録されているとする。自己位置推定部４２は、マークＭＫの位置及びカメラ１４の内部パラメータを逆変換することにより、マーク座標系上のカメラ１４の位置を推定する（例えば、http://www.hitl.washington.edu/artoolkit/documentation/tutorialcamera.htm、及び、http://im-lab.net/artoolkit-overview/参照）。

図８は、ピンホールカメラモデルによる座標系とカメラ１４の位置推定について説明する図である。図８に示すように、自己位置推定部４２は、ピンホールカメラモデルに基づいて、カメラ１４の位置及び姿勢を推定してもよい。ピンホールカメラモデルは、例えば、三次元空間と、二次元の画像平面との幾何学的な相対関係を表したモデルによって、三次元の世界座標系（図８に示すＸｗＹｗＺｗ空間）、三次元のカメラ座標系（図８に示すＸｃＹｃＺｃ空間）、及び、二次元の画像座標系（図８に示すＸｉＹｉ空間）を定義する。自己位置推定部４２は、当該モデルによって、複数の特徴点ＰＴと、キャリブレーションにより算出したカメラ１４の内部パラメータに基づいて、ＰｎＰ問題を解くことにより、外部パラメータ（世界座標系からカメラ座標系への変換行列）を算出して、カメラ１４の位置を推定する（例えば、“OpenCV3 プログラミングブック /藤本雄一郎 他/(株)マイナビ出版”の１１１頁〜参照）。

図９は、マークＭＫによるカメラ１４の位置推定における左右方向（または水平方向）の誤差を説明する図である。カメラ１４の位置推定における誤差は、ローリングシャッター現象及びモーションブラー等に起因する。図９は、ほぼ上方から見た平面図である。カメラ１４上に示すＸｃ及びＺｃは、図７におけるカメラ座標系の座標軸を示す。座標軸Ｘｃは移動体９２の左右方向を示す。座標軸Ｚｃは移動体９２の前後方向を示す。図９においては、実際のマークＭＫは、実線で示すように、カメラ１４及び移動体９２の前方の位置ＰＳ０に存在する。マークＭＫ上に示すＸｍ及びＺｍは、図７に示すマーク座標系を示す。

移動体９２に取り付けられたカメラ１４が、前方の位置ＰＳ０に存在するマークＭＫを撮影する。カメラ１４は、ローリングシャッター現象またはモーションブラー等の影響で、マークＭＫを投影面ＰＰａに示す像ＭＫａのように左右方向に歪んだ形状（例えば、菱形）のマークとして撮影する場合がある。尚、投影面ＰＰａは、撮影されたマークＭＫを投影した面である。

この場合、自己位置推定部４２は、歪んだ像ＭＫａを含む画像ＩＧによって、マークＭＫを位置ＰＳ１に存在すると誤認識して、カメラ１４を位置ＰＳ２に存在すると誤って推定する。自己位置推定部４２は、既知のマークＭＫの位置ＰＳ０を基準として、誤認識したマークＭＫの位置ＰＳ１とカメラ１４の位置ＰＳ２との位置関係から算出した一点鎖線で示すベクトルＶＣ１を適用することにより、カメラ１４が位置ＰＳ３に存在すると推定する。従って、自己位置推定部４２は、実際の位置ＰＳ４から左右方向にずれた位置ＰＳ３にカメラ１４が存在すると推定する。

自己位置推定部４２がこれらの誤って推定したカメラ１４の位置ＰＳ３の推定結果を削除するために、正しい推定結果と判定する判定範囲ＪＲ１を予め設定することが好ましい。判定範囲ＪＲ１は、予め定められた条件の一例である。自己位置推定部４２は、判定範囲ＪＲ１内と推定したカメラ１４の位置を第２自己位置として採用する。自己位置推定部４２は、判定範囲ＪＲ１外と推定したカメラ１４の位置を破棄する。判定範囲ＪＲ１は、例えば、カメラ座標系から見たカメラ１４の位置をＸｍベクトル及びＺｍベクトルで表して設定される。

図１０は、マークＭＫによるカメラ１４の位置推定における上下方向（または鉛直方向）の誤差を説明する図である。カメラ１４の位置推定における誤差は、ローリングシャッター現象及びモーションブラー等に起因する。図１０は、ほぼ右方向から見た平面図である。カメラ１４上に示すＹｃ及びＺｃは、図７におけるカメラ座標系の座標軸を示す。座標軸Ｙｃは移動体９２の上下方向を示す。座標軸Ｚｃは移動体９２の前後方向を示す。図１０においては、実際のマークＭＫは、実線で示すように、カメラ１４及び移動体９２の前方の位置ＰＳ５に存在する。マークＭＫ上に示すＹｍ及びＺｍは、図７に示すマーク座標系の座標軸を示す。

移動体９２に取り付けられたカメラ１４が、前方の位置ＰＳ５に存在するマークＭＫを撮影する。カメラ１４は、ローリングシャッター現象またはモーションブラー等の影響で、マークＭＫを投影面ＰＰｂに示す像ＭＫｂのように上下に歪んだ形状（例えば、台形）で撮影する場合がある。尚、投影面ＰＰｂは、撮影されたマークＭＫを投影した面である。

この場合、自己位置推定部４２は、マークＭＫを位置ＰＳ６に存在すると誤認識して、カメラ１４を位置ＰＳ７に存在すると誤って推定する。自己位置推定部４２は、既知のマークＭＫの位置ＰＳ５を基準として、誤認識したマークＭＫの位置ＰＳ６とカメラ１４の位置ＰＳ７との位置関係から算出したベクトルＶＣ２を適用することにより、カメラ１４が位置ＰＳ８に存在すると推定する。従って、自己位置推定部４２は、実際の位置ＰＳ９から上下方向にずれた位置ＰＳ８にカメラ１４が存在すると推定する。

自己位置推定部４２がこれらの誤って推定したカメラ１４の位置推定の結果を削除するために、正しい推定結果と判定する判定範囲ＪＲ２を予め設定することが好ましい。判定範囲ＪＲ２は、予め定められた条件の別の例である。自己位置推定部４２は、判定範囲ＪＲ２内と推定したカメラ１４の位置を第２自己位置として採用する。自己位置推定部４２は、判定範囲ＪＲ２外と推定したカメラ１４の位置を破棄する。判定範囲ＪＲ２は、例えば、カメラ座標系から見たカメラ１４の位置をＹｍベクトル及びＺｍベクトルで表して設定される。

図１１は、画像処理装置１６による自己位置推定処理のフローチャートである。

図１１に示すように、自己位置推定処理では、まず、画像処理装置１６が初期化を実行する（Ｓ１０２）。例えば、画像処理装置１６の自己位置推定部４２が、判定範囲ＪＲ１、ＪＲ２を設定する。例えば、自己位置推定部４２は、移動体９２の走行する道路９４の幅等を基準として、図９に示す判定範囲ＪＲ１のＸｃ方向の範囲を設定する。自己位置推定部４２は、移動体９２に取り付けられたカメラ１４の高さ等を基準として、図１０に示す判定範囲ＪＲ２のＹｃ方向の範囲を設定する。

画像取得部２０は、カメラ１４からフレーム画像を画像ＩＧとして取得して、フレームメモリ２２に格納する（Ｓ１０４）。画像補正部２４は、フレームメモリ２２に格納された画像ＩＧを補正して、画像メモリ２６に格納する（Ｓ１０６）。

特徴点抽出部２８は、画像メモリ２６に格納された補正後の画像ＩＧから特徴点ＰＴを抽出して、特徴点格納メモリ３０に格納する（Ｓ１０８）。特徴点追跡部３２は、追跡結果に含まれる１つ前の特徴点ＰＴからテンプレートパッチＴＰを生成して、画像メモリ２６に格納された補正後の画像ＩＧに基づいて、特徴点ＰＴを追跡する（Ｓ１１０）。特徴点追跡部３２は、特徴点ＰＴのオプティカルフローＦＬを含む追跡結果を追跡結果格納メモリ３４に格納する（Ｓ１１２）。

マーク認識部３６は、マーク情報格納メモリ３８から取得したマークＭＫのパターン情報及び位置情報に基づいて、特徴点格納メモリ３０に格納された複数の特徴点ＰＴを含む特徴点群からマークＭＫを認識して検出し、マークＭＫの認識結果とともに、マークＭＫの位置情報をマーク認識結果格納メモリ４０に格納する（Ｓ１１４）。

自己位置推定部４２は、追跡結果格納メモリ３４に格納された特徴点ＰＴのオプティカルフローＦＬによる追跡結果に基づいて、移動体９２及びカメラ１４の位置を推定して、当該位置を第１自己位置と推定する（Ｓ１１６）。自己位置推定部４２は、認識したマークＭＫの位置情報及び認識結果に基づいて、移動体９２及びカメラ１４の位置を推定して、当該位置を第２自己位置と推定する（Ｓ１１８）。

自己位置推定部４２は、予め設定された判定範囲ＪＲ１、ＪＲ２に基づいて、第１自己位置及び第２自己位置のいずれかを採用することにより、自己位置を補正する（Ｓ１２０）。例えば、自己位置推定部４２は、特徴点ＰＴを追跡したオプティカルフローＦＬによる第１自己位置を自己位置ＳＰとして採用する。この状態で、自己位置推定部４２は、マークＭＫの位置情報及び認識結果による第２自己位置が判定範囲ＪＲ１、ＪＲ２内であれば、第２自己位置を自己位置ＳＰとして採用することにより、第１自己位置による自己位置ＳＰをリセットして補正する。この後、自己位置推定部４２は、第２自己位置を採用した自己位置ＳＰを起点として、特徴点ＰＴの追跡による第１自己位置を採用して自己位置ＳＰの推定を継続する。尚、自己位置推定部４２は、第２自己位置が判定範囲ＪＲ１、ＪＲ２内であることに加えて、第１自己位置の位置ずれの大きさ、及び、第１自己位置の推定回数等によって、第２自己位置を自己位置ＳＰとして採用するか否かを判定してもよい。

この後、画像処理装置１６は、フレーム画像である画像ＩＧがまだある場合（Ｓ１２２：Ｙｅｓ）、ステップＳ１０４以降を繰り返す。画像処理装置１６は、全ての画像ＩＧに対して処理を実行した場合（Ｓ１２２：Ｎｏ）、自己位置推定処理を終了する。

上述したように、画像処理装置１６では、自己位置推定部４２が、マークＭＫに基づいて推定した第２自己位置が予め定められた条件を満たしている場合、第２自己位置を自己位置ＳＰとして採用して、第１自己位置による自己位置ＳＰを補正している。これにより、画像処理装置１６は、自己位置ＳＰの推定精度を向上させることができる。特に、画像処理装置１６では、自己位置推定部４２が、特徴点ＰＴを追跡したオプティカルフローＦＬによって推定した第１自己位置を自己位置ＳＰとして採用している状態で、マークＭＫに基づいて推定した第２自己位置が予め定められた条件を満たすと、第２自己位置によって自己位置ＳＰをリセットして補正する。これにより、画像処理装置１６は、第１自己位置による自己位置の位置ずれが大きくなっても、第２自己位置によって位置ずれをリセットしてなくすことにより、自己位置ＳＰの推定精度を向上させることができる。また、画像処理装置１６は、第２自己位置が明らかに異常の場合（例えば、移動体９２が自動車等の場合に第２自己位置が空中または地中等と推定された場合）、第２自己位置を予め定められた条件によって排除できるので、自己位置ＳＰの推定精度を向上できる。

図１２は、別の自己位置の推定を説明する図である。図１２に示す例では、第１マークＭＫ１が建造物９０の１階に設けられ、第２マークＭＫ２が建造物９０の２階に設けられている。マーク情報格納メモリ３８は、位置の基準となる基準位置Ｐ０に対するマークＭＫ１、ＭＫ２の位置に関する位置情報を格納している。例えば、マーク情報格納メモリ３８は、基準位置Ｐ０からマークＭＫ１、ＭＫ２の中心までのそれぞれの高さＨＤ１、ＨＤ２の情報を含む位置情報を格納している。画像処理装置１６は、自動車等の飛行できない移動体９２に設けられているとする。従って、画像処理装置１６は、水平方向に移動しつつ、撮影された画像ＩＧに基づいて、第２自己位置を推定する。

自己位置推定部４２は、水平方向に移動しつつ撮影された画像ＩＧから認識された第１マークＭＫ１の高さＨＤ１の情報に基づいて移動体９２ａの高さを含む第２自己位置を推定する。自己位置推定部４２は、自己位置ＳＰの高さが第１基準高さＳＨ１を中心とした第１高さ範囲ＨＡ１内に収まる場合、移動体９２ａの自己位置ＳＰが１階であることを示す情報を第２自己位置の推定結果に付加する。自己位置推定部４２は、第２マークＭＫ２の高さＨＤ２の情報に基づいて移動体９２ｂの高さを含む第２自己位置を推定する。自己位置推定部４２は、自己位置ＳＰの高さが第２基準高さＳＨ２を中心とした第２高さ範囲ＨＡ２内に収まる場合、移動体９２ｂの自己位置ＳＰが２階であることを示す情報を第２自己位置の推定結果に付加する。これにより、自己位置推定部４２は、ＧＰＳ（Global Positioning System）及びオプティカルフローＦＬ等では得られない高さ方向の情報を得ることができる。このように、自己位置推定部４２は、当該自己の高さに基づいて、マークＭＫによる第２自己位置を推定する。

自己位置推定部４２は、駐車場等の建造物９０の水平方向を走行して移動する移動体９２の自己位置ＳＰを推定する場合、基準位置Ｐ０に対する基準高さＳＨ１、ＳＨ２がマークＭＫ１、ＭＫ２からの距離に関わらずほぼ一定のため、自己位置ＳＰの高さのみを基準に第２自己位置を推定することができる。

図１３は、飛行可能な移動体９２ｃ、９２ｄの自己位置の推定を説明するための上方から見た平面図である。図１４は、飛行可能な移動体９２ｃ、９２ｄの自己位置の推定を説明するための側方から見た図である。飛行可能な移動体９２ｃ、９２ｄの一例は、ＵＡＶ（Unmanned Aerial Vehicle／Unmanned Air Vehicle）である。マークＭＫ３、ＭＫ４、ＭＫ５は、建造物９０の壁９０ａ、柱９０ｂ等に設けられている。

図１３及び図１４に示すように、第１移動体９２ｃの自己位置推定部４２は、認識したマークＭＫ１の高さ、マークＭＫ１までの距離及びマークＭＫ１の中心からのずれに基づいて、第２自己位置を推定する。自己位置推定部４２は、推定した第２自己位置が予め定められた判定範囲ＪＲ３内であれば、当該第２自己位置によって、第１自己位置を採用している自己位置ＳＰをリセットして補正する。尚、予め定められた判定範囲ＪＲ３は、Ｘ方向、Ｙ方向及びＺ方向の位置情報を含む直方体または立方体で規定される。自己位置推定部４２は、推定した第２自己位置が判定範囲ＪＲ３外であれば、第２自己位置を採用することなく、壁９０ａ及び柱９０ｂ等の画像から抽出した特徴点ＰＴに基づいて推定した第１自己位置を自己位置ＳＰとして採用する。

第２移動体９２ｄの自己位置推定部４２は、マークＭＫ２の高さ、マークＭＫ２までの距離及びマークＭＫ２の中心からのずれに基づいて第２自己位置を推定する。自己位置推定部４２は、推定した当該第２自己位置が予め定められた判定範囲ＪＲ４内の場合、第２移動体９２ｄは旋回して、認識した別のマークＭＫ３に基づいて、第２自己位置を推定してもよい。この場合、第２移動体９２ｄの自己位置推定部４２は、自己位置ＳＰが判定範囲ＪＲ４内であれば、当該第２自己位置によって自己位置ＳＰをリセットして補正してよい。これにより、画像処理装置１６は、より自己位置ＳＰの推定精度を向上できる。

図１５は、本実施形態に係る画像処理装置１６のハードウェア構成を模式的に示すブロック図である。図１５に示すように、本実施形態に係る画像処理装置１６は、ＣＰＵ（Central Processing Unit）５０と、ＲＡＭ（Random Access Memory）５２と、ＲＯＭ（Read Only Memory）５４と、ＨＤＤ（Hard Disk Drive）５６と、Ｉ／Ｆ５８と、バス６０とを備えるコンピュータである。ＣＰＵ５０、ＲＡＭ５２、ＲＯＭ５４、ＨＤＤ５６及びＩ／Ｆ５８は、バス６０を介して、接続されている。また、Ｉ／Ｆ５８にはＬＣＤ（Liquid Crystal Display）等の表示部６２及び操作部６４が接続されている。

ＣＰＵ５０はプロセッサ等の演算手段である。ＣＰＵ５０は、画像処理装置１６の制御全般を司る。ＲＡＭ５２は、情報の高速な読み書きが可能な揮発性の記憶媒体であり、ＣＰＵ５０が情報を処理する際の作業領域として用いられる。ＲＯＭ５４は、読み出し専用の不揮発性記憶媒体であり、ファームウェア等のプログラムを格納する。ＨＤＤ５６は、情報の読み書きが可能な不揮発性の記憶媒体であり、ＯＳ（Operating System）、各種の制御プログラム、及び、アプリケーション・プログラム等を格納する。

Ｉ／Ｆ５８は、バス６０と各種のハードウェアやネットワーク等を接続し制御する。表示部６２は、利用者が画像処理装置１６の状態を確認するための視覚的利用者インタフェースである。操作部６４は、キーボード、マウス、各種のハードボタン、タッチパネル等、ユーザが画像処理装置１６に情報を入力するためのユーザインタフェースである。

本実施の形態の画像処理装置１６が実行する自己位置推定プログラムは、上述した各部（画像取得部２０、フレームメモリ２２、画像補正部２４、画像メモリ２６、特徴点抽出部２８、特徴点格納メモリ３０、特徴点追跡部３２、追跡結果格納メモリ３４、マーク認識部３６、マーク情報格納メモリ３８、マーク認識結果格納メモリ４０、自己位置推定部４２）を含むモジュール構成となっている。実際のハードウェアとしてはＣＰＵ５０が上記ＲＯＭ５４から自己位置推定プログラムを読み出して実行することにより、上記各部が主記憶装置上にロードされる。これにより、画像取得部２０、フレームメモリ２２、画像補正部２４、画像メモリ２６、特徴点抽出部２８、特徴点格納メモリ３０、特徴点追跡部３２、追跡結果格納メモリ３４、マーク認識部３６、マーク情報格納メモリ３８、マーク認識結果格納メモリ４０、自己位置推定部４２が主記憶装置上に生成されて、これらの機能がコンピュータに実現されるようになっている。

例えば、本実施の形態の画像処理装置１６で実行される自己位置推定プログラムは、ＲＯＭ５４等に予め組み込まれて提供される。本実施の形態の画像処理装置１６で実行される自己位置推定プログラムは、インストール可能な形式又は実行可能な形式のファイルでＣＤ−ＲＯＭ、フレキシブルディスク（FD）、ＣＤ−Ｒ、ＤＶＤ（Digital Versatile Disk）等のコンピュータで読み取り可能な記録媒体に記録して提供するように構成してもよい。

さらに、本実施の形態の画像処理装置１６で実行される自己位置推定プログラムを、インターネット等のネットワークに接続されたコンピュータ上に格納し、ネットワーク経由でダウンロードさせることにより提供するように構成しても良い。また、本実施の形態の画像処理装置１６で実行される自己位置推定プログラムをインターネット等のネットワーク経由で提供または配布するように構成しても良い。

上述した各実施形態の構成の機能、接続関係、個数等は適宜変更してよい。上述した各実施形態は、適宜組み合わせてもよい。上述したフローチャートのステップの順序は適宜変更してよい。

例えば、画像取得部２０は、カメラ１４によって撮影された画像を取得したが、測距装置等によって撮影された測距画像等を取得してもよい。

上述の実施形態では、自己位置推定部４２が、特徴点ＰＴのオプティカルフローＦＬによる第１自己位置を採用した自己位置ＳＰを、第２自己位置によって補正する例をあげたが、実施形態はこれに限定されない。例えば、自己位置推定部４２は、第２自己位置を自己位置ＳＰとして採用しつつ、第２自己位置が判定範囲ＪＲ１、ＪＲ２外の場合に、第１自己位置を自己位置ＳＰとして採用することにより、自己位置ＳＰを補正してもよい。

上述の実施形態では、（ｎ−１）番目の特徴点ＰＴを追跡結果格納メモリ３４に格納して、テンプレートパッチＴＰを生成する例を示したが、予め（ｎ−１）番目の特徴点ＰＴに基づいてテンプレートパッチＴＰを生成して、追跡結果格納メモリ３４に格納してもよい。

上述の実施形態は、例として提示したものであり、本発明の範囲を限定することは意図していない。この新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことも可能である。実施形態および実施形態の変形は、発明の範囲や要旨に含まれると共に、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

１６…画像処理装置
３２…特徴点追跡部
３６…マーク認識部
４２…自己位置推定部
９２…移動体
ＦＬ…オプティカルフロー
ＨＤ１…高さ
ＨＤ２…高さ
ＩＧ…画像
ＭＫ…マーク
ＰＴ…特徴点
ＳＰ…自己位置

特許第４３６９４３９号公報

Claims (9)

1. 撮影された被写体の画像から抽出された前記被写体の特徴点を追跡する特徴点追跡部と、
   前記画像内の予め設けられたマークを認識するマーク認識部と、
   前記特徴点の追跡結果に基づいて第１自己位置を推定し、前記マークに基づいて第２自己位置を推定し、予め定められた条件に基づいて、前記第１自己位置及び前記第２自己位置のいずれか一方を自己位置として採用する自己位置推定部と、
   を備える画像処理装置。
2. 前記自己位置推定部は、予めパターン情報が設定された前記マークの位置情報に基づいて、前記第２自己位置を推定する
   請求項１に記載の画像処理装置。
3. 前記自己位置推定部は、水平方向に移動しつつ撮影された前記画像から前記マークを認識して、自己の高さに基づいて前記第２自己位置を推定する
   請求項１または２に記載の画像処理装置。
4. 前記自己位置推定部は、前記マークの高さ情報を含むマーク情報に基づいて、前記第２自己位置の高さを推定する
   請求項１から３のいずれか１項に記載の画像処理装置。
5. 前記自己位置推定部は、前記マークの高さ情報及び前記マークまでの距離と、前記マークの中心からのずれに基づいて、前記第２自己位置を推定する
   請求項１から４のいずれか１項に記載の画像処理装置。
6. 前記自己位置推定部は、一のマークから推定した前記第２自己位置が予め定められた範囲内の場合、他のマークに基づいて前記第２自己位置を補正する
   請求項１から５のいずれか１項に記載の画像処理装置。
7. 前記自己位置推定部は、前記第１自己位置を前記自己位置として採用している状態で、前記第２自己位置が前記予め定められた条件を満たしている場合、前記第２自己位置を前記自己位置として採用して補正する
   請求項１から６のいずれか１項に記載の画像処理装置。
8. 撮影された被写体の画像から抽出された前記被写体の特徴点を追跡する特徴点追跡段階と、
   前記画像内の予め設けられたマークを認識するマーク認識段階と、
   前記特徴点の追跡結果に基づいて第１自己位置を推定し、前記マークに基づいて第２自己位置を推定し、予め定められた条件に基づいて、前記第１自己位置及び前記第２自己位置のいずれか一方を自己位置として採用する自己位置推定段階と、
   を備える自己位置推定方法。
9. 撮影された被写体の画像から抽出された前記被写体の特徴点を追跡する特徴点追跡機能と、
   前記画像内の予め設けられたマークを認識するマーク認識機能と、
   前記特徴点の追跡結果に基づいて第１自己位置を推定し、前記マークに基づいて第２自己位置を推定し、予め定められた条件に基づいて、前記第１自己位置及び前記第２自己位置のいずれか一方を自己位置として採用する自己位置推定機能と、
   をコンピュータに実現させるプログラム。

Images