画像処理装置及び移動体位置推定方法
【課題】周囲の状況に関わらず自己の位置の推定精度を向上させる。
【解決手段】予め周囲地図記憶部１１に記憶された移動体の周囲の特徴情報２２と当該特徴情報２２の位置情報２１とを取得し、地図内面構造設定部１２が複数の特徴情報２２を含む領域の面構造を設定し、地図内特徴点可視性判断部１３が、面構造によって、移動体の位置から特徴情報２２が撮像不能である場合に、当該特徴情報を可視性がないと判定し、自己位置推定部１５が、移動体の周囲の特徴情報２２のうち可視性がないと判定された特徴情報２２を除いて、取得された特徴情報２２及び位置情報２１と自己の位置から撮像した撮像画像内の実特徴とに基づいて、移動体の位置を推定する。
【解決手段】予め周囲地図記憶部１１に記憶された移動体の周囲の特徴情報２２と当該特徴情報２２の位置情報２１とを取得し、地図内面構造設定部１２が複数の特徴情報２２を含む領域の面構造を設定し、地図内特徴点可視性判断部１３が、面構造によって、移動体の位置から特徴情報２２が撮像不能である場合に、当該特徴情報を可視性がないと判定し、自己位置推定部１５が、移動体の周囲の特徴情報２２のうち可視性がないと判定された特徴情報２２を除いて、取得された特徴情報２２及び位置情報２１と自己の位置から撮像した撮像画像内の実特徴とに基づいて、移動体の位置を推定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、撮像した画像を処理する画像処理装置、及び、画像処理に基づいて移動体の位置を推定する移動体位置推定方法に関する。
【背景技術】
【０００２】
従来より、移動体の位置を推定する技術としては、下記の特許文献１に記載されたものが知られている。この特許文献１には、移動型ロボットの自己の位置を推定する方法が記載されている。この方法は、ロボットに搭載されたカメラの画像から画像特徴量を検出し、当該検出された画像特徴量と、地図情報として予め保存された画像特徴量との相関に基づいて、ロボットの位置を推定している。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００３−２６６３４５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上述した特許文献１に記載された方法では、相関がある画像特徴量として目印にしていた物体が、例えば障害物に隠れたために撮像されなくなった場合には、自己の位置を推定する精度が低下する問題があった。
【０００５】
そこで、本発明は、上述した実情に鑑みて提案されたものであり、周囲の状況に関わらず自己の位置の推定精度を向上させることができる画像処理装置及び移動体位置推定方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明は、移動体周囲の複数の特徴情報を含む領域の面構造を設定し、当該設定された面構造によって、移動体の位置から特徴情報が撮像不能である場合に、当該特徴情報を可視性がないと判定し、可視性がないと判定された特徴情報を除いて、移動体の位置を推定する。
【発明の効果】
【０００７】
本発明によれば、可視性がない特徴情報を除いて移動体の位置を推定するので、障害物によって特徴情報が隠れるといった周囲の状況に関わらず自己の位置を推定でき、自己の位置の推定精度を向上させることができる。
【図面の簡単な説明】
【０００８】
【図１】本発明の第１実施形態として示す自己位置推定システムの機能的な構成を示すブロック図である。
【図２】本発明の第１実施形態として示す自己位置推定システムにおいて、面構造について説明する図である。
【図３】本発明の第１実施形態として示す自己位置推定システムにおいて、面構造の遮蔽によって排除される特徴情報を示す斜視図である。
【図４】本発明の第１実施形態として示す自己位置推定システムにおいて、面構造の法線方向と撮像カメラの撮像方向との関係を示す斜視図である。
【図５】本発明の第１実施形態として示す自己位置推定システムによって自己の位置を推定する処理手順を示すフローチャートである。
【図６】本発明の第２実施形態として示す自己位置推定システムにおいて、複数の物体について設定された面構造を示す斜視図である。
【図７】本発明の第２実施形態として示す自己位置推定システムにおいて、面構造を分割した分割面を示す斜視図である。
【図８】本発明の第２実施形態として示す自己位置推定システムにおいて、面構造を修正した状態を示す斜視図である。
【発明を実施するための形態】
【０００９】
以下、本発明の実施の形態について図面を参照して説明する。
【００１０】
本発明の第１実施形態として示す自己位置推定システムは、例えば図１に示すように構成される。この自己位置推定システムは、例えば、車両（以下、移動体とも言う）に搭載されている。
【００１１】
自己位置推定システムは、図１に示すように、画像処理装置１と、撮像カメラ２とを有する。
【００１２】
撮像カメラ２は、所定の方向を撮像するように移動体に取り付けられている。移動体の回転や移動に伴なって、その撮像方位が変化する撮像カメラ２は、撮像した撮像画像を画像処理装置１の画像内特徴量検出部１４に供給する。撮像カメラ２は、移動体が移動しているときには、連続して画像を撮像し、連続して当該撮像画像を画像処理装置１に供給する。
【００１３】
画像処理装置１は、周囲地図記憶部１１、地図内面構造設定部１２、地図内特徴点可視性判断部１３、画像内特徴量検出部１４、自己位置推定部１５を含む。なお、画像処理装置１は、実際にはＲＯＭ、ＲＡＭ、ＣＰＵ等にて構成されているが、当該ＣＰＵがＲＯＭに格納された自己位置推定用のプログラムに従って処理をすることによって実現できる機能をブロックとして説明する。
【００１４】
周囲地図記憶部１１は、位置情報２１と、当該位置情報２１に対応付けられた特徴情報２２とを含む周囲地図を記憶する（特徴記憶手段）。この特徴情報２２は、自己位置を推定するために用いられる画像特徴量である。この画像特徴量としては、エッジ、コーナー、色分布が含まれる。周囲地図記憶部１１は、移動体の位置の推定時に、位置情報２１及び特徴情報２２とを多数含む移動体周囲の地図を、後段の処理で利用できるようにメモリーに読み込む。
【００１５】
周囲地図とは、画像によるトラッキングが可能なコーナーなどの画像特徴量の３次元配置であり、Ｍ（Ip, x, y, z）の集合である。Ｉｐは特徴情報２２としての部分画像、ｘ，ｙ，ｚは３次元の位置情報２１である。周囲地図を取得する手法としては、移動体における異なる位置のカメラにより撮像された２以上の画像を用いたステレオマッチング手法、距離が計測できるＬＲＦや投光式計測装置と撮像装置をあわせて使用し、トラッキング可能な特徴点までの距離を計測する方法、予め人工的に周囲環境を直接的に計測する方法が挙げられる。
【００１６】
画像内特徴量検出部１４は、移動体に設けられた撮像カメラ２により撮像された撮像画像から、実特徴を抽出する（実特徴抽出手段）。この実特徴とは、移動体の周囲の状況に応じて画像内に含まれた画像特徴量としての部分画像である。この画像特徴量としては、エッジ、コーナー、色分布が含まれる。
【００１７】
自己位置推定部１５は、周囲地図記憶部１１に記憶された特徴情報２２及び位置情報２１と、画像内特徴量検出部１４により抽出された実特徴とに基づいて、移動体の位置を推定する（位置推定手段）。より具体的には、自己位置推定部１５は、自己の位置として、撮像カメラ２の位置を推定する。
【００１８】
地図内面構造設定部１２は、周囲地図記憶部１１に記憶された複数の特徴情報２２を用いて、当該複数の特徴情報２２を含む領域の面構造を設定する（面構造設定手段）。地図内面構造設定部１２は、周囲地図記憶部１１に記憶された画像特徴量の部分集合に対して、面構造を設定する。この面構造は、面状の障害物が存在する箇所に現れる。例えば、面構造は、壁、他車両の存在する箇所に位置すると設定される。本実施形態における「面構造」は、複数の特徴情報２２の位置関係が面状に離散しているときにおいて、当該位置関係から推定できる面を指しており、実在する面ではない。また、「面構造」は、特徴情報２２が厳密に面状に配置されている場合のみならず、多少の幅を持たせて定義している。この点で、単に「面」とは異なる。
【００１９】
具体的には、この地図内面構造設定部１２は、図２に示すように、周囲地図記憶部１１に、移動体の周囲の特徴情報２２として、複数の特徴情報２２ａ，２２ｂが記憶されているとする。このとき、地図内面構造設定部１２は、周囲地図記憶部１１で得られた特徴情報２２ａ，２２ｂの位置情報２１に基づいて、複数の特徴情報２２ａを含む部分集合と、複数の特徴情報２２ｂを含む部分集合を検出する。地図内面構造設定部１２は、当該部分集合の位置関係が、平面的な配置をしているかを判断する。地図内面構造設定部１２は、当該部分集合の位置関係が平面的な配置をしていると判断した場合に、当該部分集合に対して面構造３０を設定する。図２の例では、特徴情報２２ａが面構造３０ａという平面に配置されているので、複数の特徴情報２２ａを含む面構造３０ａを設定でき、特徴情報２２ｂが面構造３０ｂという平面的に配置されているので、複数の特徴情報２２ｂを含む面構造３０ｂを設定できる。図２のように、３次元空間において、直立した面構造３０としては、壁、障害物、コーナー等が挙げられる。
【００２０】
地図内特徴点可視性判断部１３は、地図内面構造設定部１２により面構造３０と自己位置推定部１５により推定されている移動体の位置との位置関係に基づいて、当該面構造３０における特徴情報２２の可視性を判断する（可視性判断手段）。地図内特徴点可視性判断部１３は、当該移動体の位置から当該面構造３０に含まれる特徴情報２２が、撮像カメラ２によって撮像不能である場合に可視性がないと判定する。一方、地図内特徴点可視性判断部１３は、当該面構造３０に含まれる特徴情報２２が、撮像カメラ２によって撮像可能である場合に可視性があると判定する。
【００２１】
具体的には、図３に示すように、撮像カメラ２の撮像位置２ａや画角等によって決定される撮像範囲に、特徴情報２２ａ，２２ｂ，２２ｃが周囲地図記憶部１１に記憶されているとする。地図内面構造設定部１２は、３つの特徴情報２２ｂを含む面構造３０を設定する。そして、地図内特徴点可視性判断部１３は、面構造３０に対して撮像カメラ２側に位置する特徴情報２２ａ及び面構造３０上の特徴情報２２ｂを可視性があると判断する。一方、地図内面構造設定部１２は、撮像カメラ２の撮像範囲であっても面構造３０によって遮蔽される特徴情報２２ｃを、可視性がないと判断する。
【００２２】
また、図４に示すように、面構造３０上の特徴情報２２に対する撮像カメラ２の撮像方向と、面構造３０の法線方向（面構造３０の向き）との角度θが９０度以上である場合には、特徴情報２２を、可視性が無いものとする。
【００２３】
このような自己位置推定システムにおいて、自己位置推定部１５は、地図内特徴点可視性判断部１３によって可視性がないと判定された特徴情報２２を除いて、移動体の位置を推定する。このために、自己位置推定部１５は、地図内特徴点可視性判断部１３により可視性があると判断された面構造３０に含まれる特徴情報２２のみを用いて、自己の位置を推定する。すなわち、自己位置推定部１５は、可視性がある特徴情報２２のみが地図内特徴点可視性判断部１３から供給され、当該可視性がある特徴情報２２と、画像内特徴量検出部１４によって検出された実特徴とを比較する。そして、自己位置推定部１５は、双方の特徴が合致する位置情報２１を取得して、当該取得した位置情報２１に基づいて、自己の位置を推定する。換言すれば、自己位置推定部１５は、地図内特徴点可視性判断部１３によって可視性がないと判定された特徴情報２２を除いて、移動体の位置を推定する。
【００２４】
このような自己位置推定システムは、移動体が移動しているときに、周囲地図記憶部１１に記憶された位置情報２１及び特徴情報２２を含む周囲地図を読み込む。そして、地図内面構造設定部１２及び地図内特徴点可視性判断部１３によって、面構造３０を定義して特徴情報２２の可視性がない部分を排除し、自己の位置を推定する。
【００２５】
つぎに、上述した自己位置推定システムにおいて、移動体が移動しているときに自己の位置を推定する具体的な処理手順について、図５を参照して説明する。
【００２６】
先ずステップＳ１において、撮像カメラ２によって撮像画像を取得する。ステップＳ１は、移動体が移動しているときに所定時間毎に行われる。
【００２７】
ステップＳ２において、画像処理装置１により、周囲地図を取得する必要性があるか否かを判断する。本実施形態においては、既に周囲地図が周囲地図記憶部１１に構築されており、移動体の位置の推定時に周囲地図記憶部１１に記憶された周囲地図をすべて読み込むものとする。したがって、ステップＳ２において、移動体の移動開始時の最初の一回だけ周囲地図を取得する必要性があると判断することとなる。
【００２８】
ステップＳ２にて肯定判定された後のステップＳ３において、周囲地図記憶部１１は、予め記憶された周囲地図を読み出す。これにより、画像処理装置１は、周囲地図を取得する。周囲地図はファイル形式で保存されているとし、画像処理装置１は、周囲地図記憶部１１を構成する記録媒体から計算メモリー内にロードする。
【００２９】
次のステップＳ４において、地図内面構造設定部１２は、面構造３０の設定を行う。このとき、地図内面構造設定部１２は、特徴情報２２の位置情報２１に基づいて、特徴情報２２を部分集合に分類（クラスタリング）する。そして、地図内面構造設定部１２は、当該部分集合が平面的に分布している場合には面構造３０を設定し、そうでない場合には、当該部分集合によって面構造３０を設定しない。
【００３０】
なお、クラスタリング手法にはさまざまな手法が存在するが、既存のあらゆるクラスタリング手法を採用できる。例えば、k-means手法、複数のカーネルを設定してそれらのパラメータを最尤推定で最適化する手法等が挙げられる。
【００３１】
また、部分集合に含まれる複数の特徴情報２２が平面状に分布しているか否かの判断は、例えば以下のように行う。先ず、各部分集合に対して、当該部分集合に含まれる特徴情報２２（特徴点）が平面（ｘ，ｙ，ｚ，ｗ）Ｔ上に分布することを仮定して、平面パラメータｘ，ｙ，ｚ，ｗを設定する。その後、当該仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔ上に特徴情報２２が分布したと仮定したときの残差（residual error：ｅｒｅｓ）を、下記の式１を用いて求める。
【数１】
【００３２】
式１において、ｎは部分集合内の特徴情報２２の数であり、ｘｔは周囲地図内の特徴情報２２の位置情報２１であり、ｘ＾ｉは、仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔに対して位置情報ｘｔから垂線を下ろしたときの接点である。また、関数ｄ（ｘ）は、各特徴情報２２の位置情報２１と接点ｘ＾ｉとの２点間距離を表す。
【００３３】
式１を用いて得られる残差ｅｒｅｓは、仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔと、特徴情報２２の位置情報２１との差異（エラー）である。そして、当該差異と、所定のしきい値ｅｔｈを比較して、当該差異がしきい値ｅｔｈよりも小さい特徴情報２２を、当該仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔに含まれるとする。しきい値ｅｔｈは、予め設定した値であり、仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔに対して特徴情報２２が分布する誤差の許容度を表す。
【００３４】
地図内面構造設定部１２は、残差ｅｒｅｓがしきい値ｅｔｈよりも小さい場合には、仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔに含まれる部分集合とする。一方、残差ｅｒｅｓがしきい値ｅｔｈよりも大きい場合には、当該特徴情報２２が当該仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔに含まれないとする。そして、地図内面構造設定部１２は、仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔを面構造３０として設定でき、当該面構造３０に含まれる特徴情報２２を特定できる。ここで、面構造３０の大きさは、分類した特徴情報２２を仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔに投影したとき、平面座標上で、ｘ，ｙの最大値、最小値により定義される大きさであるとする。
【００３５】
このように、地図内面構造設定部１２は、複数の特徴情報２２を当該複数の特徴情報２２の位置情報２１に基づいて部分集合に分類する。そして、当該部分集合に含まれる特徴情報２２と、仮想平面との差異が所定のしきい値ｅｔｈよりも小さい場合に、当該仮想平面に含まれる特徴情報２２を含む領域を面構造３０として設定することができる。
【００３６】
一方、ステップＳ２にて否定判定がされた後又はステップＳ４の後のステップＳ５において、地図内特徴点可視性判断部１３は、周囲地図内における特徴点（特徴情報２２）の可視性を判断する。可視性がある場合とは、撮像カメラ２によって特徴情報２２が撮像可能である場合である。
【００３７】
このとき、地図内特徴点可視性判断部１３は、２つの判断要素を用いて、特徴情報２２の可視性を判断する。第１の判断要素は、地図内特徴点可視性判断部１３が、面構造３０による特徴情報２２の遮蔽に基づいて特徴情報２２の可視性を判定する。第２の判断要素は、地図内特徴点可視性判断部１３が、地図内面構造設定部１２により設定された面構造３０における法線方向と撮像カメラ２の撮像方向との角度に基づいて特徴情報２２の可視性を判定する。
【００３８】
具体的には、図３に示すように、面構造３０による特徴情報２２の遮蔽に基づいて、撮像カメラ２に対して面構造３０の反対側に存在する特徴情報２２を可視性がないと判定する。更に、図４に示すように、面構造３０における法線方向と撮像カメラ２の撮像方向との角度θに基づいて、角度θが９０度以上であるときには当該面構造３０上の特徴情報２２を可視性がないと判定する。
【００３９】
また、面構造３０の法線方向と、面構造３０上の特徴情報２２を撮像カメラ２から撮像する方向との差に応じて、特徴情報２２を構成する部分画像の見え方が変化する。このため、撮像カメラ２の撮像方向と面構造３０の法線方向との角度θが９０度よりも小さい場合において、地図内特徴点可視性判断部１３は、特徴情報２２を構成する部分画像の外郭形状を、撮像カメラ２の位置に応じて変換することが望ましい。このとき、地図内特徴点可視性判断部１３は、例えば、特徴情報２２を構成する部分画像を、アフィン（Affine）変換により、下記の式４によって撮像カメラ２からの特徴情報２２の見え方にあわせて変換する。そして、地図内特徴点可視性判断部１３は、当該変換された特徴情報２２を用いて、可視性を判断する。
【００４０】
Ｉｐ’＝ＡＩｐ （式２）
ここで、Ｉｐは、周囲地図内に含まれる特徴情報２２を構成する部分画像、Ｉｐ’は撮像カメラ２の位置から観測した特徴情報２２の部分画像であって、当該特徴情報２２の変換後の画像である。
【００４１】
次のステップＳ６において、画像内特徴量検出部１４は、ステップＳ５にて可視性があると判断された特徴情報２２の撮像画像内での位置を検出する。このとき、画像内特徴量検出部１４は、撮像カメラ２から撮像画像を取得する。画像内特徴量検出部１４は、取得した撮像画像と、地図内特徴点可視性判断部１３から供給された可視性のある特徴情報２２とを比較する。画像内特徴量検出部１４は、可視性のある特徴情報２２に対応した実特徴が撮像画像に含まれている場合には、当該撮像画像中の特徴情報２２とみなされる実特徴の画像位置を、当該特徴情報２２の画像内位置として検出する。
【００４２】
撮像画像内の特徴情報２２を探索する方法は、様々な方法が提案されている。例えば、一般的に知られている、下記の式３により表されるＳＡＤ（Sum of Absolute Difference）、式４により表されるＳＳＤ（Sum of Squared intensity Difference）による画像比較を、ラスタースキャンにより撮像画像の全体に対して行い、最も特徴情報２２との一致度合いが高い撮像画像内の領域を検出位置とする。
【数２】
【００４３】
上記の式３，４において、ＭＬは撮像カメラ２により得られた撮像画像の部分画像であり、ＭＲは周囲地図内特徴情報２２である部分画像である。また、ラスタースキャンとは、撮像画像の左上から、真横に移動させて探索し、当該撮像画像の右端まで探索したら、一画素だけ下に下がる手順を繰り返しながら、撮像画像の全体を探索する順番である。
【００４４】
次のステップＳ７において、自己位置推定部１５は、ステップＳ６にて検出された特徴情報２２に対応した実特徴の撮像画像内での位置と、当該特徴情報２２の周囲地図内での位置とから、撮像カメラ２の位置を推定する。この撮像カメラ２の位置を求める方法は、多く知られている。例えば、先ず、自己位置推定部１５は、撮像カメラ２の位置を任意の値で仮置きする。この仮の撮像カメラ２の位置において、周囲地図上の特徴情報２２を周囲地図上に投影したときの特徴情報２２の位置と、実際に撮像画像内に含まれる特徴情報２２の位置との差を求める。そして、当該差を、撮像カメラ２の位置をパラメータとして最小化する。この方法は、効率の観点から望ましい。
【００４５】
以上のように、本発明の第１実施形態として示す自己位置推定システムによれば、既存の周囲の特徴情報２２を読み込み、当該特徴情報２２から面構造３０を定義し、撮像カメラ２から可視性がない特徴情報２２を排除して、自己の位置を推定することができる。したがって、この自己位置推定システムによれば、可視性がある特徴情報２２のみを使用して位置を推定できるので、障害物によって特徴情報２２が隠れるといった周囲の状況に関わらず、自己の位置の推定精度を向上させることができる。
【００４６】
例えば、駐車車両などの障害物が多く存在する場合、特徴情報２２が隠れると、自己位置の推定が継続不可能になってしまう可能性が高い。これに対し、自己位置推定システムによれば、面構造３０によって隠れる特徴情報２２を排除して自己の位置の推定するので、当該問題を回避して、自己の位置の推定をすることができる。
【００４７】
また、この自己位置推定システムによれば、面構造３０による遮蔽に基づいて特徴情報２２の可視性を判定すると共に、面構造３０の向きと撮像カメラ２の撮像方向との角度に基づいて特徴情報２２の可視性を判定するので、撮像カメラ２から見えない特徴情報２２を確実に排除できる。これにより、自己位置推定システムによれば、更に自己の位置の推定精度を高めることができる。
【００４８】
更に、この自己位置推定システムによれば、面構造３０上の特徴情報２２については、当該面構造３０の向きと、撮像カメラ２の撮像方向との角度に基づいて、当該特徴情報２２としての画像を変換する。これにより、自己位置推定システムによれば、喩え周囲地図記憶部１１に記憶されている特徴情報２２と、撮像カメラ２によって撮像された撮像画像内の特徴情報２２とが異なる形状であっても、周囲地図記憶部１１に記憶されている特徴点の形状を変換して、精度よく撮像画像内の特徴情報２２を検出できる。これにより、自己位置推定システムによれば、更に自己の位置の推定精度を高めることができる。
【００４９】
更に、この自己位置推定システムによれば、複数の特徴情報２２を当該位置情報２１に基づいて部分集合に分類し、当該部分集合に含まれる特徴情報２２と、仮想平面との差異が所定のしきい値よりも小さい場合に、当該仮想平面に含まれる特徴情報２２を含む領域を面構造３０として設定する。これにより、自己位置推定システムによれば、位置が近い特徴情報２２のうちで平面状の位置関係にある複数の特徴情報２２によって面構造３０を設定でき、当該面構造３０によって隠れる特徴情報２２を精度良く排除できる。したがって、自己位置推定システムによれば、更に自己の位置の推定精度を高めることができる。
【００５０】
［第２実施形態］
つぎに、第２実施形態に係る情報表示システムについて説明する。なお、上述の実施形態と同様の部分については同一符号を付することによりその詳細な説明を省略する。
【００５１】
第２実施形態として示す自己位置推定システムは、精度良く面構造３０を設定する点で、第１実施形態とは異なる。
【００５２】
第２実施形態の自己位置推定システムにおける地図内面構造設定部１２は、ステップＳ４において、面構造３０を複数の分割面の集合に分割する。地図内特徴点可視性判断部１３は、当該分割した複数の分割面における向きと撮像カメラ２の撮像方向との角度のみに基づいて、当該各分割面における特徴情報２２の可視性を判定する。
【００５３】
このとき、地図内面構造設定部１２は、当該分割した複数の分割面よりも、撮像カメラ２からら遠方に位置する特徴情報２２が、撮像カメラ２によって撮像されている場合に、当該分割面を面構造３０から取り除く。これにより、地図内面構造設定部１２は、面構造３０を適切に設定して、地図内特徴点可視性判断部１３は、当該設定された面構造３０に基づいて特徴情報２２の可視性を判断する。
【００５４】
第１実施形態においては、ステップＳ４にて分類した特徴情報２２の集合に対し、単一の平面からなる面構造３０を設定したが、特徴情報２２の集合が実際に単一の平面により構成されている保証は無い。例えば図６に示すように、平面状に並んだ複数の物体４０Ａ，４０Ｂが単一の面構造３０として設定される可能性がある。この場合、複数の物体４０Ａ，４０Ｂの間は何も無い空間であり、より遠方に存在する特徴情報２２が撮像カメラ２によって撮像される可能性も考えられる。
【００５５】
このような問題に対し、第２実施形態の自己位置推定システムは、ステップＳ４において、地図内面構造設定部１２によって、図７に示すように、面構造３０を、複数の分割面３０ａの集合に分割する。このとき、地図内面構造設定部１２は、図６のように設定された面構造３０を、所定の大きさの分割面３０ａに分割する。この所定の大きさは、周囲地図に含まれる特徴情報２２を含む程度の大きさであることが望ましい。具体的には、物体４０Ａ，４０Ｂのエッジ成分が含まれる部分画像（特徴情報２２）を含む程度の大きさを、分割面３０ａの大きさとする。そして、地図内面構造設定部１２は、同一の法線ベクトルを有する分割された分割面３０ａの集合として面構造３０の設定を行う。
【００５６】
このステップＳ４の後のステップＳ５では、撮像カメラ２の撮像方向と分割面３０ａの法線方向との角度のみに基づいて可視性の判断を行う。すなわち、第１実施形態のステップＳ５においては、面構造３０による遮蔽による特徴情報２２の可視性の判断と、撮像方向と面構造３０の法線方向との角度θによる可視性の判断を行っていたが、第２実施形態においては、当該角度θのみに基づいて可視性を判断する。
【００５７】
次のステップＳ６において、画像内特徴量検出部１４は、ステップＳ５で可視性があると判断された特徴情報２２が、撮像カメラ２による撮像された撮像画像内に実特徴として含まれるかを判断し、当該実特徴の撮像画像内の位置を検出する。これにより、ステップＳ６においては、面構造３０によって遮蔽される特徴情報２２についても、撮像画像内での位置を検出する。画像内特徴量検出部１４は、上述したように、ＳＡＤやＳＳＤ等により十分小さい値が得られないときは、当該特徴情報２２に対応した実特徴が撮像画像内では検出されなかったものとする。
【００５８】
このステップＳ６にて撮像画像内に実特徴として検出されない特徴情報２２は、ステップＳ７での自己の位置の推定には使用しない。すなわち、ステップＳ６にて撮像画像内に実特徴として検出されない特徴情報２２は、周囲地図上では面構造３０に遮蔽されるものとなる。
【００５９】
これに対して、周囲地図上では面構造３０によって遮蔽されると考えられる特徴情報２２が、撮像画像内で実特徴として検出された場合は、当該特徴情報２２を、ステップＳ７の自己の位置の推定に使用する。また、当該実特徴よりも撮像カメラ２に対して近い位置に設定された分割面３０ａを削除する。
【００６０】
これにより、図８に示すように、実際には撮像カメラ２から撮像可能な物体４０Ａ上の領域４０ａと撮像カメラ２との間の分割面３０ａを削除して、物体４０Ａについての面構造３０Ａと、物体４０Ｂについての面構造３０Ｂとを設定できる。
【００６１】
以上のように、本発明の第２実施形態として示す自己位置推定システムによれば、第１実施形態と同様に、既存の周囲の特徴情報２２を読み込み、当該特徴情報２２から面構造３０を定義し、撮像カメラ２から可視性がない特徴情報２２を排除して、自己の位置を推定することができる。したがって、この自己位置推定システムによれば、可視性がある特徴情報２２のみを使用して位置を推定できるので、障害物によって特徴情報２２が隠れるといった周囲の状況に関わらず、自己の位置の推定精度を向上させることができる。
【００６２】
また、この自己位置推定システムによれば、面構造３０を複数の分割面３０ａの集合に分割し、当該分割した複数の分割面３０ａにおける向きと撮像カメラ２の撮像方向との角度のみに基づいて、当該各分割面３０ａにおける特徴情報２２の可視性を判定する。これにより、自己位置推定システムによれば、面構造３０が誤って設定されており、当該面構造３０の背後の特徴情報２２が実特徴として撮像カメラ２から可視性がある場合に、当該特徴情報２２を含めて自己の位置の推定ができる。したがって、自己位置推定システムによれば、更に自己の位置の推定精度を高めることができる。
【００６３】
更に、この自己位置推定システムによれば、面構造３０を複数の分割面３０ａの集合に分割し、当該分割した複数の分割面３０ａよりも撮像カメラ２から遠方に位置する特徴情報２２が撮像カメラ２によって撮像されている場合に、当該分割面３０ａを取り除く。これにより、自己位置推定システムは、面構造３０を修正して、より現実に近い面構造３０を設定でき、更に自己の位置の推定精度を高めることができる。
【００６４】
なお上述した実施の形態においては、自己位置推定システムが、車両に搭載されているものとして説明したが、ロボットなどの移動体に搭載されても、同様の効果を有する。
【００６５】
なお、上述の実施の形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施の形態以外であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計等に応じて種々の変更が可能であることは勿論である。
【符号の説明】
【００６６】
１ 画像処理装置
２ 撮像カメラ
１１ 周囲地図記憶部
１２ 地図内面構造設定部
１３ 地図内特徴点可視性判断部
１４ 画像内特徴量検出部
１５ 自己位置推定部
２１ 位置情報
２２ 特徴情報
２２ａ 特徴情報
３０ 面構造
【技術分野】
【０００１】
本発明は、撮像した画像を処理する画像処理装置、及び、画像処理に基づいて移動体の位置を推定する移動体位置推定方法に関する。
【背景技術】
【０００２】
従来より、移動体の位置を推定する技術としては、下記の特許文献１に記載されたものが知られている。この特許文献１には、移動型ロボットの自己の位置を推定する方法が記載されている。この方法は、ロボットに搭載されたカメラの画像から画像特徴量を検出し、当該検出された画像特徴量と、地図情報として予め保存された画像特徴量との相関に基づいて、ロボットの位置を推定している。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００３−２６６３４５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上述した特許文献１に記載された方法では、相関がある画像特徴量として目印にしていた物体が、例えば障害物に隠れたために撮像されなくなった場合には、自己の位置を推定する精度が低下する問題があった。
【０００５】
そこで、本発明は、上述した実情に鑑みて提案されたものであり、周囲の状況に関わらず自己の位置の推定精度を向上させることができる画像処理装置及び移動体位置推定方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明は、移動体周囲の複数の特徴情報を含む領域の面構造を設定し、当該設定された面構造によって、移動体の位置から特徴情報が撮像不能である場合に、当該特徴情報を可視性がないと判定し、可視性がないと判定された特徴情報を除いて、移動体の位置を推定する。
【発明の効果】
【０００７】
本発明によれば、可視性がない特徴情報を除いて移動体の位置を推定するので、障害物によって特徴情報が隠れるといった周囲の状況に関わらず自己の位置を推定でき、自己の位置の推定精度を向上させることができる。
【図面の簡単な説明】
【０００８】
【図１】本発明の第１実施形態として示す自己位置推定システムの機能的な構成を示すブロック図である。
【図２】本発明の第１実施形態として示す自己位置推定システムにおいて、面構造について説明する図である。
【図３】本発明の第１実施形態として示す自己位置推定システムにおいて、面構造の遮蔽によって排除される特徴情報を示す斜視図である。
【図４】本発明の第１実施形態として示す自己位置推定システムにおいて、面構造の法線方向と撮像カメラの撮像方向との関係を示す斜視図である。
【図５】本発明の第１実施形態として示す自己位置推定システムによって自己の位置を推定する処理手順を示すフローチャートである。
【図６】本発明の第２実施形態として示す自己位置推定システムにおいて、複数の物体について設定された面構造を示す斜視図である。
【図７】本発明の第２実施形態として示す自己位置推定システムにおいて、面構造を分割した分割面を示す斜視図である。
【図８】本発明の第２実施形態として示す自己位置推定システムにおいて、面構造を修正した状態を示す斜視図である。
【発明を実施するための形態】
【０００９】
以下、本発明の実施の形態について図面を参照して説明する。
【００１０】
本発明の第１実施形態として示す自己位置推定システムは、例えば図１に示すように構成される。この自己位置推定システムは、例えば、車両（以下、移動体とも言う）に搭載されている。
【００１１】
自己位置推定システムは、図１に示すように、画像処理装置１と、撮像カメラ２とを有する。
【００１２】
撮像カメラ２は、所定の方向を撮像するように移動体に取り付けられている。移動体の回転や移動に伴なって、その撮像方位が変化する撮像カメラ２は、撮像した撮像画像を画像処理装置１の画像内特徴量検出部１４に供給する。撮像カメラ２は、移動体が移動しているときには、連続して画像を撮像し、連続して当該撮像画像を画像処理装置１に供給する。
【００１３】
画像処理装置１は、周囲地図記憶部１１、地図内面構造設定部１２、地図内特徴点可視性判断部１３、画像内特徴量検出部１４、自己位置推定部１５を含む。なお、画像処理装置１は、実際にはＲＯＭ、ＲＡＭ、ＣＰＵ等にて構成されているが、当該ＣＰＵがＲＯＭに格納された自己位置推定用のプログラムに従って処理をすることによって実現できる機能をブロックとして説明する。
【００１４】
周囲地図記憶部１１は、位置情報２１と、当該位置情報２１に対応付けられた特徴情報２２とを含む周囲地図を記憶する（特徴記憶手段）。この特徴情報２２は、自己位置を推定するために用いられる画像特徴量である。この画像特徴量としては、エッジ、コーナー、色分布が含まれる。周囲地図記憶部１１は、移動体の位置の推定時に、位置情報２１及び特徴情報２２とを多数含む移動体周囲の地図を、後段の処理で利用できるようにメモリーに読み込む。
【００１５】
周囲地図とは、画像によるトラッキングが可能なコーナーなどの画像特徴量の３次元配置であり、Ｍ（Ip, x, y, z）の集合である。Ｉｐは特徴情報２２としての部分画像、ｘ，ｙ，ｚは３次元の位置情報２１である。周囲地図を取得する手法としては、移動体における異なる位置のカメラにより撮像された２以上の画像を用いたステレオマッチング手法、距離が計測できるＬＲＦや投光式計測装置と撮像装置をあわせて使用し、トラッキング可能な特徴点までの距離を計測する方法、予め人工的に周囲環境を直接的に計測する方法が挙げられる。
【００１６】
画像内特徴量検出部１４は、移動体に設けられた撮像カメラ２により撮像された撮像画像から、実特徴を抽出する（実特徴抽出手段）。この実特徴とは、移動体の周囲の状況に応じて画像内に含まれた画像特徴量としての部分画像である。この画像特徴量としては、エッジ、コーナー、色分布が含まれる。
【００１７】
自己位置推定部１５は、周囲地図記憶部１１に記憶された特徴情報２２及び位置情報２１と、画像内特徴量検出部１４により抽出された実特徴とに基づいて、移動体の位置を推定する（位置推定手段）。より具体的には、自己位置推定部１５は、自己の位置として、撮像カメラ２の位置を推定する。
【００１８】
地図内面構造設定部１２は、周囲地図記憶部１１に記憶された複数の特徴情報２２を用いて、当該複数の特徴情報２２を含む領域の面構造を設定する（面構造設定手段）。地図内面構造設定部１２は、周囲地図記憶部１１に記憶された画像特徴量の部分集合に対して、面構造を設定する。この面構造は、面状の障害物が存在する箇所に現れる。例えば、面構造は、壁、他車両の存在する箇所に位置すると設定される。本実施形態における「面構造」は、複数の特徴情報２２の位置関係が面状に離散しているときにおいて、当該位置関係から推定できる面を指しており、実在する面ではない。また、「面構造」は、特徴情報２２が厳密に面状に配置されている場合のみならず、多少の幅を持たせて定義している。この点で、単に「面」とは異なる。
【００１９】
具体的には、この地図内面構造設定部１２は、図２に示すように、周囲地図記憶部１１に、移動体の周囲の特徴情報２２として、複数の特徴情報２２ａ，２２ｂが記憶されているとする。このとき、地図内面構造設定部１２は、周囲地図記憶部１１で得られた特徴情報２２ａ，２２ｂの位置情報２１に基づいて、複数の特徴情報２２ａを含む部分集合と、複数の特徴情報２２ｂを含む部分集合を検出する。地図内面構造設定部１２は、当該部分集合の位置関係が、平面的な配置をしているかを判断する。地図内面構造設定部１２は、当該部分集合の位置関係が平面的な配置をしていると判断した場合に、当該部分集合に対して面構造３０を設定する。図２の例では、特徴情報２２ａが面構造３０ａという平面に配置されているので、複数の特徴情報２２ａを含む面構造３０ａを設定でき、特徴情報２２ｂが面構造３０ｂという平面的に配置されているので、複数の特徴情報２２ｂを含む面構造３０ｂを設定できる。図２のように、３次元空間において、直立した面構造３０としては、壁、障害物、コーナー等が挙げられる。
【００２０】
地図内特徴点可視性判断部１３は、地図内面構造設定部１２により面構造３０と自己位置推定部１５により推定されている移動体の位置との位置関係に基づいて、当該面構造３０における特徴情報２２の可視性を判断する（可視性判断手段）。地図内特徴点可視性判断部１３は、当該移動体の位置から当該面構造３０に含まれる特徴情報２２が、撮像カメラ２によって撮像不能である場合に可視性がないと判定する。一方、地図内特徴点可視性判断部１３は、当該面構造３０に含まれる特徴情報２２が、撮像カメラ２によって撮像可能である場合に可視性があると判定する。
【００２１】
具体的には、図３に示すように、撮像カメラ２の撮像位置２ａや画角等によって決定される撮像範囲に、特徴情報２２ａ，２２ｂ，２２ｃが周囲地図記憶部１１に記憶されているとする。地図内面構造設定部１２は、３つの特徴情報２２ｂを含む面構造３０を設定する。そして、地図内特徴点可視性判断部１３は、面構造３０に対して撮像カメラ２側に位置する特徴情報２２ａ及び面構造３０上の特徴情報２２ｂを可視性があると判断する。一方、地図内面構造設定部１２は、撮像カメラ２の撮像範囲であっても面構造３０によって遮蔽される特徴情報２２ｃを、可視性がないと判断する。
【００２２】
また、図４に示すように、面構造３０上の特徴情報２２に対する撮像カメラ２の撮像方向と、面構造３０の法線方向（面構造３０の向き）との角度θが９０度以上である場合には、特徴情報２２を、可視性が無いものとする。
【００２３】
このような自己位置推定システムにおいて、自己位置推定部１５は、地図内特徴点可視性判断部１３によって可視性がないと判定された特徴情報２２を除いて、移動体の位置を推定する。このために、自己位置推定部１５は、地図内特徴点可視性判断部１３により可視性があると判断された面構造３０に含まれる特徴情報２２のみを用いて、自己の位置を推定する。すなわち、自己位置推定部１５は、可視性がある特徴情報２２のみが地図内特徴点可視性判断部１３から供給され、当該可視性がある特徴情報２２と、画像内特徴量検出部１４によって検出された実特徴とを比較する。そして、自己位置推定部１５は、双方の特徴が合致する位置情報２１を取得して、当該取得した位置情報２１に基づいて、自己の位置を推定する。換言すれば、自己位置推定部１５は、地図内特徴点可視性判断部１３によって可視性がないと判定された特徴情報２２を除いて、移動体の位置を推定する。
【００２４】
このような自己位置推定システムは、移動体が移動しているときに、周囲地図記憶部１１に記憶された位置情報２１及び特徴情報２２を含む周囲地図を読み込む。そして、地図内面構造設定部１２及び地図内特徴点可視性判断部１３によって、面構造３０を定義して特徴情報２２の可視性がない部分を排除し、自己の位置を推定する。
【００２５】
つぎに、上述した自己位置推定システムにおいて、移動体が移動しているときに自己の位置を推定する具体的な処理手順について、図５を参照して説明する。
【００２６】
先ずステップＳ１において、撮像カメラ２によって撮像画像を取得する。ステップＳ１は、移動体が移動しているときに所定時間毎に行われる。
【００２７】
ステップＳ２において、画像処理装置１により、周囲地図を取得する必要性があるか否かを判断する。本実施形態においては、既に周囲地図が周囲地図記憶部１１に構築されており、移動体の位置の推定時に周囲地図記憶部１１に記憶された周囲地図をすべて読み込むものとする。したがって、ステップＳ２において、移動体の移動開始時の最初の一回だけ周囲地図を取得する必要性があると判断することとなる。
【００２８】
ステップＳ２にて肯定判定された後のステップＳ３において、周囲地図記憶部１１は、予め記憶された周囲地図を読み出す。これにより、画像処理装置１は、周囲地図を取得する。周囲地図はファイル形式で保存されているとし、画像処理装置１は、周囲地図記憶部１１を構成する記録媒体から計算メモリー内にロードする。
【００２９】
次のステップＳ４において、地図内面構造設定部１２は、面構造３０の設定を行う。このとき、地図内面構造設定部１２は、特徴情報２２の位置情報２１に基づいて、特徴情報２２を部分集合に分類（クラスタリング）する。そして、地図内面構造設定部１２は、当該部分集合が平面的に分布している場合には面構造３０を設定し、そうでない場合には、当該部分集合によって面構造３０を設定しない。
【００３０】
なお、クラスタリング手法にはさまざまな手法が存在するが、既存のあらゆるクラスタリング手法を採用できる。例えば、k-means手法、複数のカーネルを設定してそれらのパラメータを最尤推定で最適化する手法等が挙げられる。
【００３１】
また、部分集合に含まれる複数の特徴情報２２が平面状に分布しているか否かの判断は、例えば以下のように行う。先ず、各部分集合に対して、当該部分集合に含まれる特徴情報２２（特徴点）が平面（ｘ，ｙ，ｚ，ｗ）Ｔ上に分布することを仮定して、平面パラメータｘ，ｙ，ｚ，ｗを設定する。その後、当該仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔ上に特徴情報２２が分布したと仮定したときの残差（residual error：ｅｒｅｓ）を、下記の式１を用いて求める。
【数１】
【００３２】
式１において、ｎは部分集合内の特徴情報２２の数であり、ｘｔは周囲地図内の特徴情報２２の位置情報２１であり、ｘ＾ｉは、仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔに対して位置情報ｘｔから垂線を下ろしたときの接点である。また、関数ｄ（ｘ）は、各特徴情報２２の位置情報２１と接点ｘ＾ｉとの２点間距離を表す。
【００３３】
式１を用いて得られる残差ｅｒｅｓは、仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔと、特徴情報２２の位置情報２１との差異（エラー）である。そして、当該差異と、所定のしきい値ｅｔｈを比較して、当該差異がしきい値ｅｔｈよりも小さい特徴情報２２を、当該仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔに含まれるとする。しきい値ｅｔｈは、予め設定した値であり、仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔに対して特徴情報２２が分布する誤差の許容度を表す。
【００３４】
地図内面構造設定部１２は、残差ｅｒｅｓがしきい値ｅｔｈよりも小さい場合には、仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔに含まれる部分集合とする。一方、残差ｅｒｅｓがしきい値ｅｔｈよりも大きい場合には、当該特徴情報２２が当該仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔに含まれないとする。そして、地図内面構造設定部１２は、仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔを面構造３０として設定でき、当該面構造３０に含まれる特徴情報２２を特定できる。ここで、面構造３０の大きさは、分類した特徴情報２２を仮想平面（ｘ，ｙ，ｚ，ｗ）Ｔに投影したとき、平面座標上で、ｘ，ｙの最大値、最小値により定義される大きさであるとする。
【００３５】
このように、地図内面構造設定部１２は、複数の特徴情報２２を当該複数の特徴情報２２の位置情報２１に基づいて部分集合に分類する。そして、当該部分集合に含まれる特徴情報２２と、仮想平面との差異が所定のしきい値ｅｔｈよりも小さい場合に、当該仮想平面に含まれる特徴情報２２を含む領域を面構造３０として設定することができる。
【００３６】
一方、ステップＳ２にて否定判定がされた後又はステップＳ４の後のステップＳ５において、地図内特徴点可視性判断部１３は、周囲地図内における特徴点（特徴情報２２）の可視性を判断する。可視性がある場合とは、撮像カメラ２によって特徴情報２２が撮像可能である場合である。
【００３７】
このとき、地図内特徴点可視性判断部１３は、２つの判断要素を用いて、特徴情報２２の可視性を判断する。第１の判断要素は、地図内特徴点可視性判断部１３が、面構造３０による特徴情報２２の遮蔽に基づいて特徴情報２２の可視性を判定する。第２の判断要素は、地図内特徴点可視性判断部１３が、地図内面構造設定部１２により設定された面構造３０における法線方向と撮像カメラ２の撮像方向との角度に基づいて特徴情報２２の可視性を判定する。
【００３８】
具体的には、図３に示すように、面構造３０による特徴情報２２の遮蔽に基づいて、撮像カメラ２に対して面構造３０の反対側に存在する特徴情報２２を可視性がないと判定する。更に、図４に示すように、面構造３０における法線方向と撮像カメラ２の撮像方向との角度θに基づいて、角度θが９０度以上であるときには当該面構造３０上の特徴情報２２を可視性がないと判定する。
【００３９】
また、面構造３０の法線方向と、面構造３０上の特徴情報２２を撮像カメラ２から撮像する方向との差に応じて、特徴情報２２を構成する部分画像の見え方が変化する。このため、撮像カメラ２の撮像方向と面構造３０の法線方向との角度θが９０度よりも小さい場合において、地図内特徴点可視性判断部１３は、特徴情報２２を構成する部分画像の外郭形状を、撮像カメラ２の位置に応じて変換することが望ましい。このとき、地図内特徴点可視性判断部１３は、例えば、特徴情報２２を構成する部分画像を、アフィン（Affine）変換により、下記の式４によって撮像カメラ２からの特徴情報２２の見え方にあわせて変換する。そして、地図内特徴点可視性判断部１３は、当該変換された特徴情報２２を用いて、可視性を判断する。
【００４０】
Ｉｐ’＝ＡＩｐ （式２）
ここで、Ｉｐは、周囲地図内に含まれる特徴情報２２を構成する部分画像、Ｉｐ’は撮像カメラ２の位置から観測した特徴情報２２の部分画像であって、当該特徴情報２２の変換後の画像である。
【００４１】
次のステップＳ６において、画像内特徴量検出部１４は、ステップＳ５にて可視性があると判断された特徴情報２２の撮像画像内での位置を検出する。このとき、画像内特徴量検出部１４は、撮像カメラ２から撮像画像を取得する。画像内特徴量検出部１４は、取得した撮像画像と、地図内特徴点可視性判断部１３から供給された可視性のある特徴情報２２とを比較する。画像内特徴量検出部１４は、可視性のある特徴情報２２に対応した実特徴が撮像画像に含まれている場合には、当該撮像画像中の特徴情報２２とみなされる実特徴の画像位置を、当該特徴情報２２の画像内位置として検出する。
【００４２】
撮像画像内の特徴情報２２を探索する方法は、様々な方法が提案されている。例えば、一般的に知られている、下記の式３により表されるＳＡＤ（Sum of Absolute Difference）、式４により表されるＳＳＤ（Sum of Squared intensity Difference）による画像比較を、ラスタースキャンにより撮像画像の全体に対して行い、最も特徴情報２２との一致度合いが高い撮像画像内の領域を検出位置とする。
【数２】
【００４３】
上記の式３，４において、ＭＬは撮像カメラ２により得られた撮像画像の部分画像であり、ＭＲは周囲地図内特徴情報２２である部分画像である。また、ラスタースキャンとは、撮像画像の左上から、真横に移動させて探索し、当該撮像画像の右端まで探索したら、一画素だけ下に下がる手順を繰り返しながら、撮像画像の全体を探索する順番である。
【００４４】
次のステップＳ７において、自己位置推定部１５は、ステップＳ６にて検出された特徴情報２２に対応した実特徴の撮像画像内での位置と、当該特徴情報２２の周囲地図内での位置とから、撮像カメラ２の位置を推定する。この撮像カメラ２の位置を求める方法は、多く知られている。例えば、先ず、自己位置推定部１５は、撮像カメラ２の位置を任意の値で仮置きする。この仮の撮像カメラ２の位置において、周囲地図上の特徴情報２２を周囲地図上に投影したときの特徴情報２２の位置と、実際に撮像画像内に含まれる特徴情報２２の位置との差を求める。そして、当該差を、撮像カメラ２の位置をパラメータとして最小化する。この方法は、効率の観点から望ましい。
【００４５】
以上のように、本発明の第１実施形態として示す自己位置推定システムによれば、既存の周囲の特徴情報２２を読み込み、当該特徴情報２２から面構造３０を定義し、撮像カメラ２から可視性がない特徴情報２２を排除して、自己の位置を推定することができる。したがって、この自己位置推定システムによれば、可視性がある特徴情報２２のみを使用して位置を推定できるので、障害物によって特徴情報２２が隠れるといった周囲の状況に関わらず、自己の位置の推定精度を向上させることができる。
【００４６】
例えば、駐車車両などの障害物が多く存在する場合、特徴情報２２が隠れると、自己位置の推定が継続不可能になってしまう可能性が高い。これに対し、自己位置推定システムによれば、面構造３０によって隠れる特徴情報２２を排除して自己の位置の推定するので、当該問題を回避して、自己の位置の推定をすることができる。
【００４７】
また、この自己位置推定システムによれば、面構造３０による遮蔽に基づいて特徴情報２２の可視性を判定すると共に、面構造３０の向きと撮像カメラ２の撮像方向との角度に基づいて特徴情報２２の可視性を判定するので、撮像カメラ２から見えない特徴情報２２を確実に排除できる。これにより、自己位置推定システムによれば、更に自己の位置の推定精度を高めることができる。
【００４８】
更に、この自己位置推定システムによれば、面構造３０上の特徴情報２２については、当該面構造３０の向きと、撮像カメラ２の撮像方向との角度に基づいて、当該特徴情報２２としての画像を変換する。これにより、自己位置推定システムによれば、喩え周囲地図記憶部１１に記憶されている特徴情報２２と、撮像カメラ２によって撮像された撮像画像内の特徴情報２２とが異なる形状であっても、周囲地図記憶部１１に記憶されている特徴点の形状を変換して、精度よく撮像画像内の特徴情報２２を検出できる。これにより、自己位置推定システムによれば、更に自己の位置の推定精度を高めることができる。
【００４９】
更に、この自己位置推定システムによれば、複数の特徴情報２２を当該位置情報２１に基づいて部分集合に分類し、当該部分集合に含まれる特徴情報２２と、仮想平面との差異が所定のしきい値よりも小さい場合に、当該仮想平面に含まれる特徴情報２２を含む領域を面構造３０として設定する。これにより、自己位置推定システムによれば、位置が近い特徴情報２２のうちで平面状の位置関係にある複数の特徴情報２２によって面構造３０を設定でき、当該面構造３０によって隠れる特徴情報２２を精度良く排除できる。したがって、自己位置推定システムによれば、更に自己の位置の推定精度を高めることができる。
【００５０】
［第２実施形態］
つぎに、第２実施形態に係る情報表示システムについて説明する。なお、上述の実施形態と同様の部分については同一符号を付することによりその詳細な説明を省略する。
【００５１】
第２実施形態として示す自己位置推定システムは、精度良く面構造３０を設定する点で、第１実施形態とは異なる。
【００５２】
第２実施形態の自己位置推定システムにおける地図内面構造設定部１２は、ステップＳ４において、面構造３０を複数の分割面の集合に分割する。地図内特徴点可視性判断部１３は、当該分割した複数の分割面における向きと撮像カメラ２の撮像方向との角度のみに基づいて、当該各分割面における特徴情報２２の可視性を判定する。
【００５３】
このとき、地図内面構造設定部１２は、当該分割した複数の分割面よりも、撮像カメラ２からら遠方に位置する特徴情報２２が、撮像カメラ２によって撮像されている場合に、当該分割面を面構造３０から取り除く。これにより、地図内面構造設定部１２は、面構造３０を適切に設定して、地図内特徴点可視性判断部１３は、当該設定された面構造３０に基づいて特徴情報２２の可視性を判断する。
【００５４】
第１実施形態においては、ステップＳ４にて分類した特徴情報２２の集合に対し、単一の平面からなる面構造３０を設定したが、特徴情報２２の集合が実際に単一の平面により構成されている保証は無い。例えば図６に示すように、平面状に並んだ複数の物体４０Ａ，４０Ｂが単一の面構造３０として設定される可能性がある。この場合、複数の物体４０Ａ，４０Ｂの間は何も無い空間であり、より遠方に存在する特徴情報２２が撮像カメラ２によって撮像される可能性も考えられる。
【００５５】
このような問題に対し、第２実施形態の自己位置推定システムは、ステップＳ４において、地図内面構造設定部１２によって、図７に示すように、面構造３０を、複数の分割面３０ａの集合に分割する。このとき、地図内面構造設定部１２は、図６のように設定された面構造３０を、所定の大きさの分割面３０ａに分割する。この所定の大きさは、周囲地図に含まれる特徴情報２２を含む程度の大きさであることが望ましい。具体的には、物体４０Ａ，４０Ｂのエッジ成分が含まれる部分画像（特徴情報２２）を含む程度の大きさを、分割面３０ａの大きさとする。そして、地図内面構造設定部１２は、同一の法線ベクトルを有する分割された分割面３０ａの集合として面構造３０の設定を行う。
【００５６】
このステップＳ４の後のステップＳ５では、撮像カメラ２の撮像方向と分割面３０ａの法線方向との角度のみに基づいて可視性の判断を行う。すなわち、第１実施形態のステップＳ５においては、面構造３０による遮蔽による特徴情報２２の可視性の判断と、撮像方向と面構造３０の法線方向との角度θによる可視性の判断を行っていたが、第２実施形態においては、当該角度θのみに基づいて可視性を判断する。
【００５７】
次のステップＳ６において、画像内特徴量検出部１４は、ステップＳ５で可視性があると判断された特徴情報２２が、撮像カメラ２による撮像された撮像画像内に実特徴として含まれるかを判断し、当該実特徴の撮像画像内の位置を検出する。これにより、ステップＳ６においては、面構造３０によって遮蔽される特徴情報２２についても、撮像画像内での位置を検出する。画像内特徴量検出部１４は、上述したように、ＳＡＤやＳＳＤ等により十分小さい値が得られないときは、当該特徴情報２２に対応した実特徴が撮像画像内では検出されなかったものとする。
【００５８】
このステップＳ６にて撮像画像内に実特徴として検出されない特徴情報２２は、ステップＳ７での自己の位置の推定には使用しない。すなわち、ステップＳ６にて撮像画像内に実特徴として検出されない特徴情報２２は、周囲地図上では面構造３０に遮蔽されるものとなる。
【００５９】
これに対して、周囲地図上では面構造３０によって遮蔽されると考えられる特徴情報２２が、撮像画像内で実特徴として検出された場合は、当該特徴情報２２を、ステップＳ７の自己の位置の推定に使用する。また、当該実特徴よりも撮像カメラ２に対して近い位置に設定された分割面３０ａを削除する。
【００６０】
これにより、図８に示すように、実際には撮像カメラ２から撮像可能な物体４０Ａ上の領域４０ａと撮像カメラ２との間の分割面３０ａを削除して、物体４０Ａについての面構造３０Ａと、物体４０Ｂについての面構造３０Ｂとを設定できる。
【００６１】
以上のように、本発明の第２実施形態として示す自己位置推定システムによれば、第１実施形態と同様に、既存の周囲の特徴情報２２を読み込み、当該特徴情報２２から面構造３０を定義し、撮像カメラ２から可視性がない特徴情報２２を排除して、自己の位置を推定することができる。したがって、この自己位置推定システムによれば、可視性がある特徴情報２２のみを使用して位置を推定できるので、障害物によって特徴情報２２が隠れるといった周囲の状況に関わらず、自己の位置の推定精度を向上させることができる。
【００６２】
また、この自己位置推定システムによれば、面構造３０を複数の分割面３０ａの集合に分割し、当該分割した複数の分割面３０ａにおける向きと撮像カメラ２の撮像方向との角度のみに基づいて、当該各分割面３０ａにおける特徴情報２２の可視性を判定する。これにより、自己位置推定システムによれば、面構造３０が誤って設定されており、当該面構造３０の背後の特徴情報２２が実特徴として撮像カメラ２から可視性がある場合に、当該特徴情報２２を含めて自己の位置の推定ができる。したがって、自己位置推定システムによれば、更に自己の位置の推定精度を高めることができる。
【００６３】
更に、この自己位置推定システムによれば、面構造３０を複数の分割面３０ａの集合に分割し、当該分割した複数の分割面３０ａよりも撮像カメラ２から遠方に位置する特徴情報２２が撮像カメラ２によって撮像されている場合に、当該分割面３０ａを取り除く。これにより、自己位置推定システムは、面構造３０を修正して、より現実に近い面構造３０を設定でき、更に自己の位置の推定精度を高めることができる。
【００６４】
なお上述した実施の形態においては、自己位置推定システムが、車両に搭載されているものとして説明したが、ロボットなどの移動体に搭載されても、同様の効果を有する。
【００６５】
なお、上述の実施の形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施の形態以外であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計等に応じて種々の変更が可能であることは勿論である。
【符号の説明】
【００６６】
１ 画像処理装置
２ 撮像カメラ
１１ 周囲地図記憶部
１２ 地図内面構造設定部
１３ 地図内特徴点可視性判断部
１４ 画像内特徴量検出部
１５ 自己位置推定部
２１ 位置情報
２２ 特徴情報
２２ａ 特徴情報
３０ 面構造
【特許請求の範囲】
【請求項１】
特徴情報と当該特徴情報の位置情報とを記憶する特徴記憶手段と、
移動体に設けられた撮像手段により撮像された画像から実特徴を抽出する実特徴抽出手段と、
前記特徴記憶手段に記憶された特徴情報及び位置情報と前記実特徴抽出手段により抽出された実特徴とに基づいて、移動体の位置を推定する位置推定手段と、
前記特徴記憶手段に記憶された複数の特徴情報に対応した複数の位置情報に基づいて、当該複数の特徴情報を含む領域の面構造を設定する面構造設定手段と、
移動体の位置から特徴情報が撮像不能である場合に、当該特徴情報に可視性がないと判定する可視性判断手段とを備え、
前記位置推定手段は、前記可視性判断手段によって可視性がないと判定された特徴情報を除いて、前記移動体の位置を推定すること
を特徴とする画像処理装置。
【請求項２】
前記可視性判断手段は、前記面構造による特徴情報の遮蔽に基づいて前記特徴情報の可視性を判定すると共に、前記面構造設定手段により設定された面構造の向きと前記撮像手段の撮像方向との角度に基づいて前記特徴情報の可視性を判定することを特徴とする請求項１に記載の画像処理装置。
【請求項３】
前記面構造設定手段は、前記設定した面構造を複数の面の集合に分割し、当該分割した面の向きと前記撮像手段の撮像方向との角度のみに基づいて、当該各面における特徴情報の可視性を判定することを特徴とする請求項２に記載の画像処理装置。
【請求項４】
前記面構造設定手段は、前記設定した面構造を複数の面の集合に分割し、当該分割した面よりも、前記撮像手段から遠方に位置する特徴情報が、前記撮像手段によって撮像されている場合に、当該面を取り除くことを特徴とする請求項３に記載の画像処理装置。
【請求項５】
前記可視性判断手段は、前記面構造上の特徴情報については、当該面構造の向きと、前記撮像手段の撮像方向との角度に基づいて、当該特徴情報としての画像を変換することを特徴とする請求項１に記載の画像処理装置。
【請求項６】
前記面構造設定手段は、複数の特徴情報を位置情報に基づいて部分集合に分類し、当該部分集合に含まれる特徴情報の位置と、仮想平面の位置との差が所定のしきい値よりも小さい場合に、当該仮想平面に含まれる特徴情報を含む領域を面構造として設定することを特徴とする請求項１乃至請求項５の何れか一項に記載の画像処理装置。
【請求項７】
予め記憶手段に記憶された移動体の周囲の特徴情報と当該特徴情報の位置情報とを取得し、
前記取得された複数の特徴情報を含む領域の面構造を設定し、
前記設定された面構造によって、前記移動体の位置から特徴情報が撮像不能である場合に、当該特徴情報を可視性がないと判定し、
前記移動体の周囲の特徴情報のうち前記可視性がないと判定された特徴情報を除いて、前記取得された特徴情報及び位置情報と前記自己の位置から撮像した撮像画像内の実特徴とに基づいて、移動体の位置を推定すること
を特徴とする移動体位置推定方法。
【請求項１】
特徴情報と当該特徴情報の位置情報とを記憶する特徴記憶手段と、
移動体に設けられた撮像手段により撮像された画像から実特徴を抽出する実特徴抽出手段と、
前記特徴記憶手段に記憶された特徴情報及び位置情報と前記実特徴抽出手段により抽出された実特徴とに基づいて、移動体の位置を推定する位置推定手段と、
前記特徴記憶手段に記憶された複数の特徴情報に対応した複数の位置情報に基づいて、当該複数の特徴情報を含む領域の面構造を設定する面構造設定手段と、
移動体の位置から特徴情報が撮像不能である場合に、当該特徴情報に可視性がないと判定する可視性判断手段とを備え、
前記位置推定手段は、前記可視性判断手段によって可視性がないと判定された特徴情報を除いて、前記移動体の位置を推定すること
を特徴とする画像処理装置。
【請求項２】
前記可視性判断手段は、前記面構造による特徴情報の遮蔽に基づいて前記特徴情報の可視性を判定すると共に、前記面構造設定手段により設定された面構造の向きと前記撮像手段の撮像方向との角度に基づいて前記特徴情報の可視性を判定することを特徴とする請求項１に記載の画像処理装置。
【請求項３】
前記面構造設定手段は、前記設定した面構造を複数の面の集合に分割し、当該分割した面の向きと前記撮像手段の撮像方向との角度のみに基づいて、当該各面における特徴情報の可視性を判定することを特徴とする請求項２に記載の画像処理装置。
【請求項４】
前記面構造設定手段は、前記設定した面構造を複数の面の集合に分割し、当該分割した面よりも、前記撮像手段から遠方に位置する特徴情報が、前記撮像手段によって撮像されている場合に、当該面を取り除くことを特徴とする請求項３に記載の画像処理装置。
【請求項５】
前記可視性判断手段は、前記面構造上の特徴情報については、当該面構造の向きと、前記撮像手段の撮像方向との角度に基づいて、当該特徴情報としての画像を変換することを特徴とする請求項１に記載の画像処理装置。
【請求項６】
前記面構造設定手段は、複数の特徴情報を位置情報に基づいて部分集合に分類し、当該部分集合に含まれる特徴情報の位置と、仮想平面の位置との差が所定のしきい値よりも小さい場合に、当該仮想平面に含まれる特徴情報を含む領域を面構造として設定することを特徴とする請求項１乃至請求項５の何れか一項に記載の画像処理装置。
【請求項７】
予め記憶手段に記憶された移動体の周囲の特徴情報と当該特徴情報の位置情報とを取得し、
前記取得された複数の特徴情報を含む領域の面構造を設定し、
前記設定された面構造によって、前記移動体の位置から特徴情報が撮像不能である場合に、当該特徴情報を可視性がないと判定し、
前記移動体の周囲の特徴情報のうち前記可視性がないと判定された特徴情報を除いて、前記取得された特徴情報及び位置情報と前記自己の位置から撮像した撮像画像内の実特徴とに基づいて、移動体の位置を推定すること
を特徴とする移動体位置推定方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１１−２４８４４１（Ｐ２０１１−２４８４４１Ａ）
【公開日】平成２３年１２月８日（２０１１．１２．８）
【国際特許分類】
【出願番号】特願２０１０−１１８２５２（Ｐ２０１０−１１８２５２）
【出願日】平成２２年５月２４日（２０１０．５．２４）
【出願人】（０００００３９９７）日産自動車株式会社 (16,386)
【Ｆターム（参考）】
【公開日】平成２３年１２月８日（２０１１．１２．８）
【国際特許分類】
【出願日】平成２２年５月２４日（２０１０．５．２４）
【出願人】（０００００３９９７）日産自動車株式会社 (16,386)
【Ｆターム（参考）】
[ Back to top ]