JP3880702B2 - 画像のオプティカルフロー検出装置及び移動体の自己位置認識システム - Google Patents

Description

【０００１】
【発明の属する技術分野】
本発明は、画像のオプティカルフローをハードウエア回路で高速に検出する画像のオプティカルフロー検出装置、及び、撮像画像をリアルタイムで処理して画像のオプティカルフローを高速に検出し、移動体の自己位置を認識する移動体の自己位置認識システムに関する。
【０００２】
【従来の技術】
従来より、無人ロボット、自律走行作業車、無人ヘリコプター等、自律的に移動する移動体に関し、移動制御、経路検出、航路検出、ロケーション検出等の各種技術が開発されており、これらの中でも、自己位置認識は重要な技術の一つである。
【０００３】
この自己位置認識の技術として、例えば、自律走行作業車等の地上を自律的に走行する移動体では、振動ジャイロや光ジャイロによって２次元の角速度を検出するとともに、対地速度を測定するセンサによって並進速度を検出し、基準位置からの移動量を演算して自己位置を測定する技術があり、また、無人ヘリコプター等の飛行移動体では、慣性航法装置により、重力加速度を検出して飛行体の加速度を検出し、この加速度を積分することで移動量を知る技術がある。
【０００４】
さらに、近年では、画像処理の技術を適用して移動体の自己位置を認識する技術が開発されており、特に、移動体にカメラを撮像して周囲環境を撮像し、撮像タイミングの異なる２枚の画像間のオプティカルフローを求めて移動体の運動を検出することで自己位置を認識する技術では、膨大な情報量を有する画像ならではの周囲環境の分析が可能となり、複雑な地形を判別して正確な自律航法を実現することができる。
【０００５】
【発明が解決しようとする課題】
しかしながら、画像のオプティカルフローによって移動体の運動を検出する場合、従来、撮像画像を取り込んでリアルタイムに自己位置を認識するにはワークステーション並の処理能力が必要であり、装置の大型化や重量増を招く。
【０００６】
このため、自律移動体に搭載可能な従来の装置では、撮像画像を取り込んだ後にオフラインで処理する等して対処しなければならず、無人ヘリコプター等のように実時間で自己位置を認識する必要のある移動体への適用は困難で、適用範囲が極めて限定されていた。
【０００７】
本発明は上記事情に鑑みてなされたもので、リアルタイムでオプティカルフローを高速に検出可能とする画像のオプティカルフロー検出装置、及び、大型化することなく移動体に搭載可能で、撮像画像をリアルタイムで処理して画像のオプティカルフローから移動体の自己位置を認識することのできる移動体の自己位置認識システムを提供することを目的としている。
【０００８】
【課題を解決するための手段】
請求項１記載の発明は、異なるタイミングで撮像した２枚の画像の一方を基準画像として他方を比較画像とし、互いの対応位置を探索して画像間のオプティカルフローを求める画像のオプティカルフロー検出装置であって、ステレオカメラを有する撮像部と、上記ステレオカメラで撮像した２枚の画像における対応位置を探索して対象物までの距離に応じて生じる画素ズレ量を求め、この画素ズレ量から得られる対象物までの遠近情報を数値化した距離画像を生成する距離画像生成部と、上記距離画像の階調データ及び度数を、それぞれラッチに保持し、新たに入力された階調データと保持した階調データとを比較する比較器からの一致出力により、保持した階調データの度数を積算してヒストグラムデータを生成するヒストグラム生成部と、上記ヒストグラム生成部で生成したヒストグラムデータを参照し、基準画像の中から特徴的な絵柄の領域を第１ブロックとして複数箇所抽出するとともに、比較画像の中から上記第１ブロックに対応する領域を第２ブロックとして探索するための探索範囲を設定する画像処理部と、上記基準画像の第１ブロックの画素毎の輝度データと上記比較画像の第２ブロックの画素毎の輝度データとを、それぞれ所定バイト数のデータ系列に並び変え、各データ系列をリング状に転送するデータ転送バッファと、上記転送バッファから転送された２系統のデータ間の差分の絶対値を複数の演算器で並列に演算した後、各演算器の出力を複数段の加算器で直列に加算してシティブロック距離を計算し、このシティブロック距離が最小値となるアドレスを、上記基準画像の第１ブロックに対する上記比較画像の対応位置として出力する演算処理回路とを備えたことを特徴とする。
【０００９】
請求項２記載の発明は、移動体に搭載されて遠方風景を撮像する２台１組のステレオカメラ及び下方風景を撮像する２台１組のステレオカメラを有する撮像部と、上記ステレオカメラで撮像した２枚の画像における対応位置を探索して対象物までの距離に応じて生じる画素ズレ量を求め、この画素ズレ量から得られる対象物までの遠近情報を数値化した距離画像を生成する距離画像生成部と、上記距離画像の階調データ及び度数を、それぞれラッチに保持し、新たに入力された階調データと保持した階調データとを比較する比較器からの一致出力により、保持した階調データの度数を積算してヒストグラムデータを生成するヒストグラム生成部と、上記距離画像から上記ヒストグラム生成部で生成したヒストグラムデータを参照し、撮像画像の中から航法演算に適した特徴的な絵柄の領域を第１ブロックとして複数箇所抽出するとともに、この第１ブロックを抽出した画像とは撮像タイミングが異なる画像の中から上記第１ブロックに対応する領域を第２ブロックとして探索するための探索範囲を設定する画像処理部と、上記第１ブロックの画素毎の輝度データと上記第２のブロックの探索範囲の画素毎の輝度データとを、それぞれ所定バイト数のデータ系列に並び変え、各データ系列をリング状に転送するデータ転送バッファと、この転送バッファから転送された２系統のデータ間の差分の絶対値を複数の演算器で並列に演算した後、各演算器の出力を複数段の加算器で直列に加算してシティブロック距離を計算し、このシティブロック距離が最小値となるアドレスを出力する演算処理回路とを有するオプティカルフロー処理部と、遠方風景の撮像画像に対する上記オプティカルフロー処理部からの出力と上記距離画像とに基づいて上記第１ブロックから上記第２ブロックへのオプティカルフローを求めて移動体のフレーム間の回転速度成分を算出した後、下方風景の撮像画像に対する上記オプティカルフロー処理部からの出力と上記距離画像とに基づいて上記第１ブロックから上記第２ブロックへのオプティカルフローを求めて移動体のフレーム間の速度成分を算出し、この速度成分から上記回転速度成分を除去したフレーム間の並進速度成分を測距開始地点から見た並進速度成分に変換し、この変換した並進速度成分を累積して３次元空間における航法軌跡を求めることにより、移動体の自己位置を認識する航法処理部とを備えたことを特徴とする。
【００１０】
すなわち、請求項１記載の発明による画像のオプティカルフロー検出装置では、ステレオカメラで撮像した２枚の画像における対応位置を探索して対象物までの距離に応じて生じる画素ズレ量を求め、この画素ズレ量から得られる対象物までの遠近情報を数値化した距離画像を生成するとともに、この距離画像の階調データ及び度数を、それぞれラッチに保持し、新たに入力された階調データと保持した階調データとを比較する比較器からの一致出力により、保持した階調データの度数を積算してヒストグラムデータを生成し、このヒストグラムデータを参照して基準画像の中から特徴的な絵柄の領域を第１ブロックとして複数箇所抽出するとともに、比較画像の中から第１ブロックに対応する領域を第２ブロックとして探索するための探索範囲を設定する。そして、基準画像の第１ブロックの画素毎の輝度データ、比較画像の第２ブロックの画素毎の輝度データを、それぞれデータ転送バッファに入力すると、このデータ転送バッファで、それぞれ所定バイト数のデータ系列に並び変えて各データ系列をリング状に演算処理回路に転送し、演算処理回路で、２系統のデータ間の差分の絶対値を複数の演算器で並列に演算した後、各演算器の出力を複数段の加算器で直列に加算してシティブロック距離を計算し、このシティブロック距離が最小値となるアドレスを基準画像の所定領域に対する比較画像の対応位置として出力する。従って、この２枚の画像間の対応位置の探索結果により、オプティカルフローを求めることができる。
【００１１】
また、請求項２記載の発明による移動体の自己位置認識システムでは、遠方風景及び下方風景をそれぞれのステレオカメラで撮像し、距離画像生成部で、ステレオカメラによる２枚の画像における対応位置を探索して対象物までの距離に応じて生じる画素ズレ量を求め、この画素ズレ量から得られる対象物までの遠近情報を数値化した距離画像を生成するとともに、ヒストグラム生成部で、この距離画像の階調データ及び度数を、それぞれラッチに保持し、新たに入力された階調データと保持した階調データとを比較する比較器からの一致出力により、保持した階調データの度数を積算してヒストグラムデータを生成する。
【００１２】
そして、画像処理部で、距離画像のヒストグラムデータを参照して撮像画像の中から航法演算に適した特徴的な絵柄の領域を第１ブロックとして複数箇所抽出するとともに、この第１ブロックを抽出した画像とは撮像タイミングが異なる画像の中から第１ブロックに対応する領域を第２ブロックとして探索するための探索範囲を設定する。
【００１３】
次いで、オプティカルフロー処理部において、遠方風景の撮像画像、下方風景の撮像画像に対し、第１ブロックの画素毎の輝度データと第２のブロックの探索範囲の画素毎の輝度データとを、データ転送バッファによってそれぞれ所定バイト数のデータ系列に並び変え、各データ系列をリング状に演算処理回路に転送すると、演算処理回路では、２系統の輝度データ間の差分の絶対値を複数の演算器で並列に演算した後、各演算器の出力を複数段の加算器で直列に加算してシティブロック距離を計算し、このシティブロック距離が最小値となるアドレスを出力する。
【００１４】
そして、航法処理部で、遠方風景の撮像画像に対するオプティカルフロー処理部からの出力と距離画像とに基づいてオプティカルフローを求め、移動体のフレーム間の回転速度成分を算出するとともに、下方風景の撮像画像に対するオプティカルフロー処理部からの出力と距離画像とに基づいてオプティカルフローを求めて移動体のフレーム間の速度成分を算出し、この速度成分から回転速度成分を除去したフレーム間の並進速度成分を測距開始地点から見た並進速度成分に変換すると、この変換した並進速度成分を累積して３次元空間における航法軌跡を求め、移動体の自己位置を認識する。
【００１５】
【発明の実施の形態】
以下、図面を参照して本発明の実施の形態を説明する。図面は本発明の実施の一形態に係わり、図１は自己位置認識システムの全体構成図、図２はカメラアセンブリの構成図、図３はステレオ処理部のブロック図、図４はオプティカルフロー処理部のブロック図、図５はヒストグラム処理回路の構成図、図６は自己加算型階調・度数モジュールの回路構成図、図７はデータ転送バッファの構成図、図８は演算処理回路の構成図、図９はステレオ処理の説明図、図１０はヒストグラム処理の説明図、図１１は自己加算型階調・度数モジュールのタイムチャート、図１２はオプティカルフロー処理の説明図、図１３はＮｏ１ブロック群取得処理ルーチンのフローチャート、図１４は回転演算処理ルーチンのフローチャート、図１５は並進演算処理ルーチンのフローチャート、図１６は航法演算処理ルーチンのフローチャートである。
【００１６】
図１は、３次元空間計測用の２組のステレオカメラからなるカメラアセンブリ１０と、このカメラアセンブリ１０によって周囲環境を一定時間毎に撮像した画像間のオプティカルフロー（フレーム間の撮像座標面での動きを表した移動ベクトルの分布）に基づいて自己位置を認識する自己位置認識装置２０とからなる移動体の自己位置認識システムである。この自己位置認識システムは、本形態においては、例えば農薬散布等に使用される無人ヘリコプターに搭載され、上記カメラアセンブリ１０で撮像した画像をリアルタイムで処理して航法軌跡データを図示しない飛行制御用コンピュータに送信し、予め設定された絶対座標の航路、あるいは地上の目標物に対して設定された航路に沿ってのヘリコプターの対地高度や飛行方向の精密制御を可能としている。
【００１７】
上記自己位置認識装置２０には、上記カメラアセンブリ１０で撮像した画像をステレオ処理するステレオ処理部３０、時系列的な撮像画像間のオプティカルフローを求めるオプティカルフロー処理部６０、このオプティカルフロー処理部６０で求めたオプティカルフローに基づいてフレーム間移動量、移動体（ヘリコプター）の回転・並進成分を演算し、航法軌跡を計算する航法処理部９０が備えられており、上記ステレオ処理部３０及び上記オプティカルフロー処理部６０をハードウエア回路で構成するとともに、上記航法処理部９０を複数個のＲＩＳＣプロセッサを並列動作させるマルチプロセッサ構成とし、高速処理によるオンラインでの航法データ取得が可能となっている。
【００１８】
上記カメラアセンブリ１０は、移動体に搭載されて周囲環境を一定時間毎に撮像し、周囲環境の変化を１画面単位で捕らえるとともに、水平ズレに応じた距離情報を取得するための測距センサ的役割を果たすものであり、図２に示すように、架台１１に、移動体の回転速度成分を演算するために必要な遠方風景を撮像するための１組の遠方用ステレオカメラ１２と、移動体の並進速度成分を演算するのに必要な下方風景（地表面）を撮像するための１組の下方用ステレオカメラ１３とが設置されている。
【００１９】
上記遠方用ステレオカメラ１２は、互いに同期が取れ、且つ、シャッタースピード可変のメインカメラ（基準カメラ）１２ａ及びサブカメラ１２ｂからなり、メインカメラ（基準カメラ）１２ａとサブカメラ１２ｂとが基線長ＬSの関係で、且つ、互いの撮像面垂直軸が平行となるよう配置されている。
【００２０】
同様に、上記下方用ステレオカメラ１３も、互いに同期が取れ、且つ、シャッタースピード可変のメインカメラ（基準カメラ）１３ａ及びサブカメラ１３ｂからなり、メインカメラ（基準カメラ）１３ａと、このメインカメラ１３ａと同じ仕様のサブカメラ１３ｂとが基線長ＳSの関係で、且つ、互いの撮像面垂直軸が平行となるように配置されている。尚、遠方用ステレオカメラ１２、下方用ステレオカメラ１３間も互いに同期が取れている。
【００２１】
本形態の自己位置認識システムでは、遠方風景の撮像画像及び下方風景の撮像画像のそれぞれについて、オプティカルフローすなわち撮像時間（タイミング）の異なる画像間の動きから移動体の運動を検出するようにしており、遠方画像の動き、下方画像の動きを、それぞれの距離画像に基づいて実空間での移動量に換算し、下方画像の動きによる速度成分から遠方画像の動きによる回転速度成分を除去して純並進速度成分を求めた後、測距開始地点（スタート地点）から見た並進速度成分に変換して累積することにより、３次元空間における航法軌跡を求めて移動体の自己位置を認識する。
【００２２】
この場合、遠方用ステレオカメラ１２によって移動体の回転速度を検出し、下方用ステレオカメラ１３によって移動体の回転・並進速度を検出するためには、遠方用ステレオカメラ１２の撮像面垂直軸と下方用ステレオカメラ１３の撮像面垂直軸とが互いに直交し、且つ、遠方のメインカメラ軸と下方のメインカメラ軸とが同一平面で互いの基準点が一致していることが理想的である。
【００２３】
しかしながら、２つのメインカメラ１２ａ，１３ａの基準点を同じにすることは現実的には困難であるため、遠方用ステレオカメラ１２の撮像面垂直軸と上記下方用ステレオカメラ１３の撮像面垂直軸とが互いに直交するよう配置するとともに、遠方用のメインカメラ１２ａと下方用のメインカメラ１３ａとを寄せた配置とし、一方のカメラの３軸が他方のカメラの３軸のいずれかと平行関係になるよう配置することで、２組のステレオカメラ１２，１３から得られる遠方画像及び下方画像の動きに対し、１つの実空間座標系での取り扱いを可能とする。
【００２４】
この場合、下方用ステレオカメラ１３に３軸中心を置くが、遠方用ステレオカメラ１２が下方用ステレオカメラ１３の周囲を回転し、両者のオフセット量が生じても、後述するように、遠方画像の性質から、遠方画像の動きには影響せず、下方のカメラ軸と遠方のカメラ軸とが直交関係にあることから、並進と回転を含む速度成分から回転速度成分を除く処理や原点（測距開始地点で空間に固定したＸＹＺ座標系原点）から見た航法計算処理を簡素化し、画像の動きを的確に把握することを可能としている。
【００２５】
尚、上記カメラアセンブリ１０に使用するカメラは、ＥＩＡ準拠の白黒カメラの他、カラーＣＣＤカメラ（３−ＣＣＤ方式をも含む）、赤外線カメラ、暗視カメラ等のエリア・スキャンを行う方式のカメラや、撮像素子からの情報をデジタルで出力する方式のカメラ等が使用可能である。
【００２６】
上記ステレオ処理部３０は、上記カメラアセンブリ１０で撮像したアナログ画像をデジタル化し、メインカメラ１２ａ（１３ａ）で撮像した画像（メイン画像）とサブカメラ１２ｂ（１３ｂ）で撮像した画像（サブ画像）とのステレオマッチングにより、メインカメラ１２ａ（１３ａ）の撮像対象領域と同じパターンを持つ領域箇所をサブカメラ１２ｂ（１３ｂ）の撮像座標から探して撮像素子から対象物までの距離に応じて生じる画素のズレ（＝視差）を求め、このズレ量から得られる対象物までの遠近情報を数値化した３次元画像情報（距離画像）を取得する。
【００２７】
図３は、上記ステレオ処理部３０の詳細構成を示し、ゲインコントロールアンプ（ＧＣＡ）３１ａを有し、カメラアセンブリ１０からの画像信号を処理するアナログＩ／Ｆ（インターフェース）３１、このアナログＩ／Ｆ３１からの出力をＡ／Ｄ変換するＡ／Ｄコンバータ３２、画素毎の輝度の歪みを補正するためのシェーディング補正データをストアするＲＯＭ及びＲＡＭからなるシェーディング補正用メモリ３３、上記ＧＣＡ３１ａ及び上記Ａ／Ｄコンバータ３２への制御電圧を発生するためのＤ／Ａコンバータ３４、画像の補正・変換やカメラ制御等のデジタル処理のための各種機能をゲートによって構成するＦＰＧＡ（Field Programmable Gate Array）３５、補正・変換後の遠方・下方のメイン画像及びサブ画像をストアする画像メモリ３６、カメラシャッター制御用に画像データをストアするシャッター制御用元画像メモリ３７、オプティカルフロー用に画像データをストアするオプティカルフロー用メモリ３８、メイン画像とサブ画像とのステレオマッチングを行って距離画像を生成するステレオマッチング回路３９、距離画像をストアする距離画像メモリ４０、後述するヒストグラム処理を行うための距離データをストアするヒストグラム用距離データメモリ４１等から構成されている。
【００２８】
上記ＦＰＧＡ３５内部に構成されるデジタル信号処理のための各種機能としては、上記Ｄ／Ａコンバータ３４を制御するＤ／Ａコントローラ４５、各カメラのイメージセンサの出力特性の相違等に起因する相互の画像信号のバラツキを補正するために画像信号のゲインやオフセット量を変更するためのルックアップテーブル（ＬＵＴ；ＲＯＭで構成される）４６、このＬＵＴ４６で補正された画像データと上記シェーディング補正用メモリ３３からのシェーディング補正データとを乗算する乗算器４７、画像の明暗部を対数変換して感度調整を行うためのログ（Ｌｏｇ）変換テーブル（ＲＯＭで構成される）４８、アドレスコントローラ４９、上記シェーディング補正用メモリ３３から上記乗算器４７へのシェーディング補正データの転送を制御するシェーディングコントローラ５０、各カメラ１２ａ，１２ｂ，１３ａ，１３ｂのシャッタースピード制御等を行うカメラコントローラ５１、ＦＰＧＡ３５内部の各種パラメータを外部から書き換えあるいは読み出すための外部Ｉ／Ｆ５２等がある。
【００２９】
一方、上記オプティカルフロー処理部６０には、図４に示すように、航法演算処理に適正な画像領域の絵柄箇所（以下、Ｎｏ１ブロックと称する）を評価するためのヒストグラムを作成するヒストグラム処理回路７０、及び、撮像時間の異なる画像においてＮｏ１ブロックの絵柄と同じ箇所（以下、Ｎｏ２ブロックと称する）を探索するオプティカルフロー処理回路８０をゲートによって構成したＦＰＧＡ６１、ヒストグラムデータをストアするヒストグラムメモリ６２、Ｎｏ１ブロックのアドレスやＮｏ２ブロックのスキャン開始アドレスをストアするオプティカルフローアドレスメモリ６３が備えられている。
【００３０】
上記Ｎｏ１ブロックは、上記航法処理部９０において決定され、適度に小さい領域で画像が多少回転しても平行移動のみで個々のＮｏ１ブロックに対してＮｏ２ブロックを確実にマッチングすることができ、また、膨大な情報量の中から測距点までの距離を確実に絞れるものとされる。本形態では、Ｎｏ１ブロックの大きさは、１２×６ピクセルの小領域である。
【００３１】
上記ヒストグラム処理回路７０は、Ｎｏ１ブロックの候補である１２×６ピクセルの領域に対し、対応する距離画像の領域に出現する距離画像値のヒストグラムを作成する。このヒストグラムは、上記航法処理部９０でのＮｏ１ブロックの決定に際して参照され、撮像画像の信頼性が評価される。すなわち、距離画像は、左右のカメラが捕らえた画像パターンが同じならば、走査線方向のズレ量に応じた距離画像値が得られるため、この特徴を利用し、同じ距離画像値が周辺に所定の数があれば、対象画像は実在する確かな画像であると判断する。
【００３２】
上記ヒストグラム処理回路７０の構成は図５に示され、並列接続された＃１〜＃ｎのｎ個の自己加算型階調・度数モジュール７１、各階調・度数モジュール７１の動作を制御する制御回路７２、有効データの度数を予め算出する前処理回路７３から構成される。尚、上記階調・度数モジュール７１は、入力画像におけるサンプル数と階調数とのうちの大きい方の数に見合うだけの数が使用される。
【００３３】
それぞれの階調・度数モジュール７１は、図６に示すように、階調ラッチ７４、比較器による一致検出回路７５、ＯＲ回路７６、度数ラッチ７７、加算器７８によって構成されている。上記階調ラッチ７４のＤ入力端子、ＣＫ端子、Ｅ端子、ＳＥＴ端子には、それぞれ、上記ヒストグラム用距離データメモリ４１からの距離データ（階調データ）、同期クロック、上記制御回路７２からのロード信号（ラッチイネーブル信号）、初期化を行うクリア信号が入力され、上記一致検出回路７５の各入力端子に、上記ヒストグラム用距離データメモリ４１からの階調データと上記階調ラッチ７４のＱ端子からのラッチデータとが入力される。
【００３４】
また、上記度数ラッチ７７のＤ入力端子には、上記前処理回路７３からの度数データと上記度数ラッチ７７のＱ端子からのラッチデータとを上記加算器７８によって加算した値が入力され、上記度数ラッチ７７のＣＫ端子、Ｅ端子、ＣＬ端子には、それぞれ、同期クロック、上記ＯＲ回路７６の出力、上記制御回路７２からのクリア信号が入力される。上記ＯＲ回路７６には、上記一致検出回路７５からの一致判定信号が入力されるとともに、上記制御回路７２からのロード信号が入力される。そして、上記一致検出回路７５の一致判定信号が上記制御回路７２に入力され、上記階調ラッチ７４のＱ端子から階調データが出力されるとともに上記度数ラッチ７７のＱ端子から度数データが出力され、上記ヒストグラムメモリ６２にストアされる。
【００３５】
また、上記ＦＰＧＡ６１内部に構成されるオプティカルフロー処理回路８０は、メイン画像のＮｏ１ブロックと次フレームのメイン画像のＮｏ２ブロックの探索エリアから、それぞれ１画素毎の輝度データを読み込み、所定のバイト数のデータ系列に並び替えて次段に転送するデータ転送バッファ８１、このデータ転送バッファ８１から転送された２系統のデータ系列に対してシティブロック距離を計算し、このシティブロック距離が最小値となる位置をＮｏ２ブロックの探索結果として出力する演算処理回路８５等から構成される。
【００３６】
上記データ転送バッファ８１は、図７に示すように、シティブロック距離を計算する際の基準画像であるメイン画像と比較画像であるサブ画像とに対応する基準側バッファ８１ａと比較側バッファ８１ｂとからなり、１バイトずつデータを読み込んで６バイトのデータ系列に並び替え、次段の演算処理回路８５に転送する。
【００３７】
上記基準側バッファ８１ａは、図７（ａ）に示すように、１バイトバッファに６バイトバッファが接続された入力段８２と、１２セットの６バイトバッファをリング状に接続したリングバッファ８３とから構成され、このリングバッファ８３先頭の出力バッファ８３ａからデータを上記演算処理回路８５にリング状に転送する。また、上記比較側バッファ８１ｂは、図７（ｂ）に示すように、同様に１バイトバッファに６バイトバッファが接続された入力段８２と、１１セットの６バイトバッファをリング状に接続したリングバッファ８４とから構成され、リングが一周したら最初のデータは破棄し、次のデータ系列を読み込んで、同様に、先頭の出力バッファ８４ａからデータを上記演算処理回路８５にリング状に転送する。
【００３８】
すなわち、バッファセットをリング状に構成することにより、バッファセットから演算処理回路８５に転送するリソースを１／１２にすることが可能となり、６×１２のデータセレクタを不要とすることができる。
【００３９】
上記演算処理回路８５は、１２画素×６画素＝７２画素のシティブロック距離を計算する必要があるため、図８に示すように、６個の絶対値演算器８６と６個の加算器８７とをピラミッド状に接続したパイプライン構造のシティブロック距離計算回路８５ａに、上記絶対値演算器８６と同様の演算器にラッチを組み合わせた最小値検出回路８５ｂを接続した構成となっている。
【００４０】
シティブロック距離計算回路８５ａでは、パイプライン構造の１段目が６個の絶対値演算器８６からなる絶対値計算ブロック、２段〜４段目が、それぞれ、３個の加算器８７からなる第１加算ブロック、１個の加算器８７からなる第２加算ブロック、１個の加算器８７からなる第３加算ブロック、５段目（最終段）が１個の加算器８７からなる総和加算器となっており、６画素分を同時に入力して並列計算を行った後に連続計算を行うパラレル・シリアル型計算回路により処理速度と回路規模の両立を図っている。
【００４１】
次に、上記航法処理部９０は、ヒストグラム処理結果によるＮｏ１ブロックの決定やオプティカルフローエリアの選定（Ｎｏ２ブロックのスキャン開始位置の設定）等を行う画像処理プロセッサ系ＳＹＳ１と、オプティカルフロー処理結果に基づくフレーム間移動量、機体の回転・並進成分の演算、航法軌跡の算出等を行う航法処理プロセッサ系ＳＹＳ２とに大別される。
【００４２】
上記画像処理プロセッサ系ＳＹＳ１には、上記データ転送バッファ８１への基準側画像データとしてＮｏ１ブロックのメイン画像データをストアするＮｏ１ブロックメイン画像データメモリ９１が備えられ、航法処理プロセッサ系ＳＹＳ２には、上記演算処理回路８５の演算結果をストアするオプティカルフローデータメモリ９２が備えられている。
【００４３】
以上の構成による自己位置認識システムでは、電源投入によるシステム起動時、ハードウエア各部が初期化されるとともに、ステレオカメラの仕様定数値、航法処理変数値等の変数値が初期化される。同時に、シェーディングコントローラ５０によってシェーディング補正用メモリ３３のシェーディング補正データがＲＯＭからＲＡＭに転送される。
【００４４】
次に、システムクロックに基づく水平同期信号及び垂直同期信号が各カメラ１２ａ，１２ｂ，１３ａ，１３ｂに外部同期信号として供給され、上記航法処理部９０によって書き込まれているシャッタースピード値でＦＰＧＡ３５のカメラコントローラ５１からカメラアセンブリ１０の各カメラに別々にシャッタートリガ信号が出力される。
【００４５】
そして、ステレオ処理部３０において、ステレオカメラ１２ａ，１２ｂで撮像された遠方画像、ステレオカメラ１３ａ，１３ｂで撮像された下方画像に対し、それぞれ、２台のカメラのバラツキの補正や感度バラツキの補正がなされた後、画像メモリ３６にストアされるとともに、ステレオマッチングにより距離画像が生成され、この距離画像からオプティカルフローに使用可能な小領域（Ｎｏ１ブロック）を評価するためのヒストグラムがオプティカルフロー処理部６０のヒストグラム処理回路７０で作成される。
【００４６】
遠方画像、下方画像にそれぞれについてヒストグラムが作成されると、航法処理部９０では、作成されたヒストグラムを参照して遠方メイン画像、下方メイン画像のそれぞれについてＮｏ１ブロックを決定し、また、Ｎｏ２ブロックの探索開始アドレスを決定する。このＮｏ１ブロックの先頭アドレス、Ｎｏ２ブロックの探索開始アドレスは、オプティカルフロー処理部６０のオプティカルフローアドレスメモリ６３に書き込まれる。
【００４７】
オプティカルフロー処理部６０では、オプティカルフロー処理回路８０によって、Ｎｏ２ブロックの探索開始アドレスからスタートして、順次、対応する領域のＮｏ１ブロックとのシティブロック距離を計算し、シティブロック距離が最小値を取る領域をＮｏ２ブロックとして、その領域のアドレスを出力する。
【００４８】
これにより、航法処理部９０では、下方画像のＮｏ１ブロックとＮｏ２ブロックとの間の移動量から移動体（ヘリコプター）の速度成分（回転と並進とを含む速度成分）を演算し、この速度成分から下方画像のＮｏ１ブロックとＮｏ２ブロックとの間の移動量から算出される回転速度成分を差し引いて純並進速度成分を求める。そして、この純並進速度成分を累積して航法軌跡を計算する。
【００４９】
以上の処理のタイミングは、自己位置認識システムの内部カウンタであるラインＮｏで全て管理されており、このラインＮｏは、画像サンプルの終了時にリセットされる。本形態では、システムクロックにより駆動されて００Ｈからカウントアップされ、８２Ｈから９ＣＨまでの区間で次フレームの画像がサンプルされ、その終了時（９ＣＨ）にリセットされる。
【００５０】
次に、撮像画像をサンプルした後の各部の動作を詳細に説明する。まず、アナログＩ／Ｆ３１では、Ｄ／Ａコンバータ３４からのコントロール電圧に応じてＧＣＡ３１ａで画像信号のゲインを調整し、各ステレオカメラ１２，１３毎の２台のカメラのゲインのバラツキを調整してＡ／Ｄコンバータ３２に出力する。Ａ／Ｄコンバータ３２では、Ｄ／Ａコントローラ４５からのリファレンス電圧によって各ステレオカメラ１２，１３毎に２台のカメラの画像信号のオフセット差を調整し、アナログ画像を所定の輝度階調（例えば２５６階調のグレースケール）のデジタル画像に変換する。尚、上記ＧＣＡ３１ａのゲインコントロール電圧、Ａ／Ｄコンバータ３２のリファレンス電圧は、航法処理部９０によってＦＰＧＡ３５内部に書き込まれている電圧値がＤ／Ａコントローラ４５に転送される。
【００５１】
上記アナログＩ／Ｆ３１からＡ／Ｄコンバータ３２を経て各ステレオカメラ１２，１３毎にゲイン・オフセットの整合が取られたデジタル画像データは、ＦＰＧＡ３５内部のＬＵＴ４６へアドレスデータとして与えられ、ＬＵＴ４６からのデータとシェーディングコントローラ５０からの指示で転送されるシェーディング補正用メモリ３３からのシェーディング補正データとが乗算器４７で乗算される。これにより、各ステレオカメラ１２，１３毎に更に厳密にゲイン及びオフセットが整合されるとともに、各ステレオカメラ１２，１３の光学系に発生するシェーディング現象による輝度低下が補正される。
【００５２】
次に、乗算器４７から出力される画像データがログ変換テーブル４８によって対数変換されて明るさに比例するノイズ成分の影響が除去され、画像メモリ３６に遠方メイン画像、遠方サブ画像、下方メイン画像、下方サブ画像のデータが、次段のステレオマッチング回路３９での処理に適応した形態でストアされる。この画像メモリ３６にストアされる画像データは、水平ラインの４ライン毎のデータであり、同時に、水平４ラインに１ライン分の画像データがシャッター制御用元画像メモリ３７にストアされるとともに、オプティカルフロー用メモリ３８に遠方・下方のメイン画像データが全ラインストアされる。
【００５３】
その後、画像メモリ３６の元画像データがステレオマッチング回路３９に４バイトずつ転送され、４×４画素の領域についてメイン画像とサブ画像とのステレオマッチングが１００画素ずれまで行われ、図９に示すような距離画像が生成されて距離画像メモリ４０及びヒストグラム用距離データメモリ４１に書き込まれる。尚、ステレオカメラの撮像画像から距離画像を生成する処理については、本出願人による特開平５−１１４０９９号公報に詳述されている。
【００５４】
以上のステレオ処理により距離画像を取得すると、オプティカルフロー処理部６０では、図１０に示すように、Ｎｏ１ブロックを元画像から抽出するための１２×６画素の領域の距離データのヒストグラムをヒストグラム処理回路７０で作成する。このヒストグラム作成処理は、４００×２００画素の距離画像に対して縦４画素・横４画素ずつずらして行われ、水平１ラインについて９８個、全体で横９８×縦４９個の領域について行われる。
【００５５】
詳細には、ヒストグラム処理回路７０では、まず、クリア信号により、データをラッチする階調・度数モジュール７１の番号を指定するための制御回路７２内のモジュール番号カウンタが０に初期化され、また、全ての階調・度数モジュール７１内の度数ラッチ７７が０にされるとともに、階調ラッチ７４が設定値（入力階調データの最大値が６４Ｈの場合、ＦＦＨ）に初期化される。また、最初の４×４画素の領域のデータの度数が前処理回路７３でカウントされる。
【００５６】
そして、初期化後、最初の４×４画素の小領域の代表階調データ（距離データ）が入力されると、制御回路７２内部のモジュール番号カウンタが１とされ、＃１の階調・度数モジュール７１#1（以下、モジュール番号を添え字#1〜#nによって示す）内の階調ラッチ７４#1にロード信号が出力されるとともにＯＲ回路７６#1を介して度数ラッチ７７#1にロード信号が出力される。これにより、階調ラッチ７４#1がイネーブルとなって同期クロックにより最初の階調データが書き込まれてラッチされ、また、度数ラッチ７７#1がイネーブルとなって同期クロックにより前処理後の度数データが度数ラッチ７７に書き込まれる。
【００５７】
次に、２番目の小領域の階調データが階調・度数モジュール７１#1に入力されると、階調・度数モジュール７１#1では、内部の階調ラッチ７４#1でラッチした最初の小領域の階調データと２番目の小領域の階調データとを一致検出回路７５#1で比較し、一致判定信号をＯＲ回路７６#1及び制御回路７２に出力する。
【００５８】
この一致判定信号は、データが一致しているときに１、不一致のときに０の信号であり、一致検出回路７５#1の出力が１のとき、すなわち、最初の小領域の階調データと２番目の小領域の階調データとが一致するとき、ＯＲ回路７６#1を介して度数ラッチ７７#1がイネーブルとなり、この度数ラッチ７７#1でラッチしている度数データと今回入力された度数データとが加算器７８で加算されて度数ラッチ７７#1に書き込まれる。
【００５９】
一方、一致検出回路７５#1の出力が０のとき、すなわち、ラッチしている最初の小領域の階調データと今回入力された２番目の小領域の階調データとが一致しないときには、ＯＲ回路７６#1を介して度数ラッチ７７#1がディスエイブルの状態のままラッチデータが保持される一方、制御回路７２では内部のモジュール番号カウンタを２とし、＃２の階調・度数モジュール７１#2にロード信号を出力する。これにより、＃２の階調・度数モジュール７１#2内部の階調ラッチ７４#2及び度数ラッチ７７#2がイネーブルとなり、階調ラッチ７４#2に２番目の小領域の階調データが書き込まれるとともに、度数ラッチ７７#1に２番目の小領域の度数データが書き込まれる。
【００６０】
以上の動作を繰り返し、１つの１２×６の領域に対する処理が終了すると、＃１の階調・度数モジュール７１#1から順に階調及び度数データを読み出してヒストグラムメモリ６２にストアしてゆき、度数が０のモジュールで動作を停止してクリア信号による再初期化を行い、次の１２×６の領域の処理に移る。
【００６１】
すなわち、図１１のタイムチャートに示すように、階調データと前処理で計算した度数データとを各モジュールの入力データとして各モジュールでラッチしておき、入力された階調データが過去にラッチした階調データと一致する場合、そのモジュールでラッチしている度数データに新たな度数データを加算して再ラッチすることで度数データを蓄積する。
【００６２】
一方、過去に階調データがラッチされたどのモジュールでもデータが一致しない場合には、制御回路７２内部のモジュール番号カウンタがカウントアップされて新しいモジュールにロード信号が出力され、新しい階調データ及び度数データが新しいモジュールにラッチされる過程を繰り返し、入力画像の全ての種類の階調データ及び前処理後の度数データが次々とラッチされ、度数データが蓄積されてゆく。
【００６３】
以上のヒストグラム処理回路７０で作成されたヒストグラムは、航法処理部９０の画像処理プロセッサ系ＳＹＳ１で参照され、遠方メイン画像、下方メイン画像のそれぞれについてＮｏ１ブロックが決定される。このＮｏ１ブロックの決定は、図１３に示すＮｏ１ブロック群取得処理ルーチンに従って行われる。
【００６４】
このルーチンでは、まず、ステップS30で、取り込んだ撮像画像に対し、遠方処理をすべきか下方処理をすべきかを選択する。この処理の選択は、撮像画像の処理タイミングが遠方画像が先で下方画像が次であることから、先に取り込んだ画像は遠方画像であるとしてステップS31以降の遠方処理へ進み、次に取り込む画像に対してはステップS33以降の下方処理へ進む。
【００６５】
ステップS31以降の遠方処理では、ステップS31で、遠方画像に対してＮｏ１ブロックを探す走査範囲をイニシャル処理として設定する。この走査範囲は、例えば、撮像面の水平走査方向をＷ、垂直走査方向をＴとする全領域Ｗ×Ｔの内、水平走査方向の画面両端の検索しろｉ2と、垂直走査方向の画面両端の検索しろｊ2とを除いた領域ＷＡ×ＴＳＬ（ＷＡ＝Ｗ−２・ｉ2、ＴＳＬ＝Ｔ−２・ｊ2）とし、水平走査方向の幅ＷＡを均等に１０分割して１０個の探索エリアＷＳ×ＴＳＬを設定する。すなわち、各探索エリアＷＳ×ＴＳＬ毎に最大１個のＮｏ１ブロックを抽出するようにし、同じ探索エリアから２個以上のＮｏ１ブロックを抽出することなく、撮像画面の特定の領域に抽出箇所が集中しないようにする。
【００６６】
次に、ステップS32で、探索エリアＷＳ×ＴＳＬの中から１２×６ピクセルの絵柄箇所を切り出し、切り出し箇所の画像品質を評価し、Ｎｏ１ブロックを決定する。１２×６ピクセルの絵柄箇所の画像品質は、対応する距離画像の１２×６ピクセルの領域のヒストグラムによって良否を判定する。
【００６７】
すなわち、１２×６ピクセルの距離画像領域の距離情報にバラツキがあると、これから定めようとするＮｏ１ブロックは、ある１箇所（１点）を見ているのではなく、同じ窓（小領域）の中に遠近が異なる複数箇所の対象物を見ていることになり、ヒストグラム中に遠近が異なる複数の対象物がノイズとして出現することになる。従って、ヒストグラム中から最も出現頻度が多い距離画像値ｍａｘｚと、その最大ヒストグラム値ｍａｘｎｕｍを求め、この最大ヒストグラム値ｍａｘｎｕｍ近辺の他の距離画像値の出現頻度を調べることにより、使用する領域の対象物の候補を定めることができる。
【００６８】
本形態では、ｍａｘｚ±２の範囲に絞って１２×６ピクセルの距離画像領域に含まれる距離画像値と、それぞれのヒストグラム値を調査するようにしており、ｍａｘｚ±２の範囲の出現頻度の総度数ｄｓｕｍを以下の(1)式で算出し、所定の閾値に定める許容範囲内にある「ＯＫ情報」の度数が所定の数を満足し、且つ、ｍａｘｚ±２の範囲の総度数ｄｓｕｍが「ＯＫ情報」の度数の内の大部分を占める（例えば、８０％以上）とき、切り出した１２×６ピクセルの絵柄箇所は信頼性のある画像と判断し、Ｎｏ１ブロックとして決定する。
ｄｓｕｍ＝ｍ２＋ｍ１＋ｍａｘｎｕｍ＋ｐ１＋ｐ２ …(1)
但し、ｍ２：ｍａｘｚ−２における出現頻度
ｍ１：ｍａｘｚ−１における出現頻度
ｐ１：ｍａｘｚ＋１における出現頻度
ｐ２：ｍａｘｚ＋２における出現頻度
【００６９】
次に、Ｎｏ１ブロックの絵柄抽出が決定すると、航法計算に必要な対象物までの距離値を正確に知るため、１２×６ピクセルの距離画像領域の平均距離画像値ｆｚ1を以下の(2)式によって求める。

【００７０】
以上の遠方画像に対するＮｏ１ブロック取得処理を、各探索エリアＷＳ×ＴＳＬ毎に行い、画像全体で最大１０個のＮｏ１ブロックを取得すると、ステップS33以降の下方処理に移行する。尚、探索エリアＷＳ×ＴＳＬで適正なＮｏ１ブロックが見つからないときには、該当する探索エリアでの抽出を中止する。
【００７１】
下方処理では、ステップS33において、下方画像に対するイニシャル処理として、Ｎｏ１ブロックを探す走査範囲を設定する。この下方画像の走査範囲は、遠方画像の１０個の探索エリアＷＳ×ＴＳＬに対し、垂直走査方向をＴＡとした領域ＷＳ×ＴＡで、画面の中央ラインＴｈから進行方向側に所定量オフセットさせたラインＴｔを探索開始ラインとする領域であり、画面全体に分散した特徴的な絵柄を抽出できるよう、垂直走査方向の幅ＴＡを３分割して３０個の探索エリアＷＳ×ＴＳＧを設定する。
【００７２】
この場合、上記オフセットラインＴｔは、前回処理で求めた並進速度によって可変するようにしており、下方画像の動きを考慮し、前・後進速度に応じて撮像面を有効に利用するようにしている。
【００７３】
次に、ステップS34では、探索エリアＷＳ×ＴＳＧの中から１２×６ピクセルの絵柄箇所を切り出し、切り出し箇所の画像品質を評価してＮｏ１ブロックを決定する。この下方画像におけるＮｏ１ブロックの決定は、前述した遠方画像におけるＮｏ１ブロックの決定と同様であるが、「ＯＫ情報」を選別するための閾値が異なる。すなわち、下方画像の動きから並進速度成分を計算するため、距離画像データが多く存在する領域で、且つ、バラツキが小さい平面的な領域をＮｏ１ブロックの候補として閾値を定める。
【００７４】
そして、上記ステップS34において、水平走査方向に最大１０個のＮｏ１ブロックを取得すると、ステップS35へ進んで下方画像における全ての探索エリアの処理が済んだか否かを調べ、全ての処理が終了していない場合には、ステップS34へ戻って、一段下の探索エリアに対する処理を続行する。そして、このステップS34→ステップS35→ステップS34のループを３回繰り返して３０個の探索エリアについての処理を行う。この場合においても、探索エリアＷＳ×ＴＳＧで適正なＮｏ１ブロックが見つからないときには、該当する探索エリアでの抽出を中止する。
【００７５】
以上により、遠方画像・下方画像のそれぞれについてＮｏ１ブロック群を取得すると、画像処理プロセッサ系ＳＹＳ１では、さらに、Ｎｏ１ブロックと同じ絵柄を持つＮｏ２ブロックを探すための探索開始エリアを、所定時間経過後のＮｏ１ブロックの所在を予測して設定する。
【００７６】
すなわち、Ｎｏ１ブロックが決定すると、Ｎｏ１ブロックの撮像面での代表座標が決まるため、まず、Ｎｏ１ブロックの代表座標を中心として、上下左右に所定のオフセット量（ｘｆ，ｙｆ）を与えた４点ａ，ｂ，ｃ，ｄを決定する。そして、この４点ａ，ｂ，ｃ，ｄを結んだ４角形の枠内すなわち（２・ｘｆ）×（２・ｙｆ）の領域を、画面に動きが無いときの撮像情報でＮｏ１ブロックと同じ絵柄が存在すると予測した探索エリアとし、この探索エリアを、遠方・下方画像のそれぞれの特徴を考慮して配置する。
【００７７】
遠方画像の場合には、測定点から十分遠く（例えば、３０ｍ以上）離れた箇所を測距するため、短時間での上下・左右の並進移動による画像の変化を受け難く、遠方画像で得たＮｏ１ブロックの動きは、回転成分による動きとして捉えられる。従って、遠方画像では、撮像面座標の原点Ｏの回りの回転角αによって探索エリアの４点ａ，ｂ，ｃ，ｄの座標値を変換し、４点ａ，ｂ，ｃ，ｄを結んだ４角形の探索エリアを設定する。
【００７８】
一方、下方画像の場合には、撮像面上の移動量は、並進成分、回転成分のどちらの動きで生じたものか判断し難い。このため、一定時間経過後のＮｏ１ブロックの所在を予測し、その移動先に、次フレーム以降のＮｏ２ブロックの検索エリアを設定する。すなわち、ヘリコプターの動きに対して処理速度が十分に速いときや、実撮像範囲が広いときには、画面毎の画像の動きは大きくないため、探索エリアは単位時間当たりの最大変化量を探索範囲と置けば良いが、ヘリコプターが大きく動く場合には、処理速度や撮像範囲が十分でないと仮定して動きを予測し、過去の速度成分を参考にして探索エリアを設定する。
【００７９】
一定時間経過後のＮｏ１ブロックの所在を予測するには、前回の角速度計算で得られたロール・ピッチ・ヨー成分と前回の並進速度計算で得られた純並進量とから、所定時間後のＮｏ１ブロックの移動先を撮像面座標の動き（仮想的なオプティカルフロー）に変換する。そして、Ｎｏ１ブロックを決定したフレームとＮｏ２ブロックを探し出す対象フレームとの間の仮想的なオプティカルフローに基づいて、Ｎｏ２ブロックの探索エリアを設定する。
【００８０】
以上により決定されたＮｏ１ブロックの画像データは、画像処理プロセッサ系ＳＹＳ１のＮｏ１ブロックメイン画像データメモリ９１にストアされ、Ｎｏ１ブロックのアドレスとＮｏ２ブロックの探索開始アドレスとがオプティカルフロー処理部６０のオプティカルフローアドレスメモリ６３にストアされる。
【００８１】
オプティカルフロー処理部６０では、Ｎｏ１ブロックメイン画像データメモリ９１からのデータと次のフレームでサンプルされてオプティカルフローメモリ３８にストアされているデータとをオプティカルフロー処理回路８０のデータ転送バッファ８１に読み込み、演算処理回路８５でシティブロック距離を計算してＮｏ２ブロックを探索する。
【００８２】
すなわち、図１２に示すように、時刻ｔ＝ｔ1における画像で決定したＮｏ１ブロックの１２×６画素のデータをａi,j（ｉ＝０〜１１，ｊ＝０〜５）、時刻ｔ＝ｔ1＋Δｔにおける画像のＮｏ２ブロックの検索エリアの１２×６画素のデータをｂi,j（ｉ＝０〜１１，ｊ＝０〜５）として、以下の(3)式に示すシティブロック距離Ｈが最小値を取る位置がＮｏ２ブロックの位置すなわちＮｏ１ブロックの移動後の位置となる。尚、(3)式中、最初のΣはｊ＝０〜５の総和、次のΣはｉ＝０〜１１の総和を示す。
Ｈ＝ΣΣ│ａi,j−ｂi,j│ …(3)
【００８３】
このシティブロック距離を演算するため、データ転送バッファ８１では、基準側バッファ８１ａにＮｏ１ブロックメイン画像データメモリ９１から１バイトずつ画像データを読み込むとともに、比較側バッファ８１ｂにオプティカルフローメモリ３８から１バイトずつ画像データを読み込み、６バイトのバッファセットが揃ったとき、それぞれ、リングバッファ８３，８４に転送する。
【００８４】
各リングバッファ８３，８４では、１番目のバッファセットが演算処理回路８５に転送され、同時に、次段のバッファにも転送される。この動作を１２回繰り返して演算処理回路８５にデータを転送すると、１つのシティブロック距離が演算される。
【００８５】
リングが１周して次のシティブロック距離演算に入ると、基準側バッファ８１ａでは、１２セットのリングバッファ８３の先頭バッファである出力バッファ８３ａに１番目のデータが格納され、比較側バッファ８１ｂでは、１１セットのリングバッファ８４の先頭バッファである出力バッファに２番目のデータが格納される（１番目のデータは破棄されている）。今度は、リングを回転させるだけで、比較側の２番目から１２番目のデータが演算処理回路８５に送られ、最後にリングを回している間に読み込んだ１３番目のデータを演算処理回路８５に転送することができる。
【００８６】
演算処理回路８５では、１２×６＝７２画素のシティブロック距離を演算するため、シティブロック距離計算回路８５ａの１段目の６個の絶対値演算器８６に６画素分のデータを入力し、輝度差の絶対値を計算する。すなわち、基準画像側の画素の輝度から比較画像側の画素の輝度を引き算し、結果が負になった場合、２の補数から絶対値の計算を行なう。
【００８７】
次に、初段を通過すると、２段目では、３個の加算器８７の各々で、１段目の絶対値演算器８６からの２つの同時入力データを加算する。このとき、同時に１段目の６個の絶対値演算器８６では、次の６画素分の絶対値計算を行う。同様に、３段目、４段目で前段の出力を加算し、５段目では、４段目の出力値と自身の出力値とを加算して総和を取る。そして、５段目で１２加算が終了すると、６×１２＝７２画素分のシティブロック距離の演算値が出力される。
【００８８】
以上の動作をＮｏ２ブロックの探索開始アドレスを１つずつインクリメントしてシティブロック距離を演算し、シティブロック距離の演算値と、このシティブロック距離の演算値が最小値を取ったときのアドレスを航法処理プロセッサ系ＳＹＳ２のオプティカルフローデータメモリ９２に出力する。そして、１つの領域についての処理を終了すると、オプティカルフローアドレスメモリ６３から次回のＮｏ１ブロックのアドレスを参照し、オプティカルフローメモリ３８からＮｏ１ブロックメイン画像データメモリ９１に次回のＮｏ１ブロックの画像データを転送する。
【００８９】
次に、航法処理プロセッサ系ＳＹＳ２において、オプティカルフロー処理結果を用いて回転・並進速度が演算され、この演算結果に基づく航法軌跡から自己位置が認識される。すなわち、航法処理プロセッサ系ＳＹＳ２では、遠方メイン画像のオプティカルフローに基づいて実距離成分の差を求め、この実距離成分の差から回転速度成分（ロール・ピッチ・ヨーの角速度）を算出するとともに、下方メイン画像のオプティカルフローに基づいて実距離成分の差を求め、この実距離成分の差による速度成分から回転速度成分を除いて３次元方向（Ｘ，Ｙ，Ｚ方向）の純並進速度を求める。そして、この並進速度を原点から見た３次元方向の速度に変換し、イニシャル地点から速度成分を累積して原点からの移動量や現在位置を割り出す。
【００９０】
遠方画像の動きから回転速度成分を演算する処理は、図１４の回転演算処理ルーチンによって行われる。このルーチンでは、まず、ステップS51で、Ｎｏ１ブロック群の配列パターンとオプティカルフロー処理部６０によって探索されたＮｏ２ブロック群の配列パターンとを比較し、オプティカルフローの信頼性を評価する。
【００９１】
すなわち、遠方画像の性質から、回転速度成分によるＮｏ１ブロック→Ｎｏ２ブロックの理想的な画像の動きでは、各ブロックの相対位置関係（配列パターン）は殆ど変わらない。従って、Ｎｏ１ブロック群の配列パターンとＮｏ２ブロック群の配列パターンとを比較し、各ブロックの配置が明らかに異なる場合には、該当するブロックをＮｏ１・Ｎｏ２ブロック群について共に排除する。
【００９２】
この配列パターンの評価は、Ｎｏ１ブロック群における各ブロック間の距離及び各ブロックで構成する三角形の面積を、Ｎｏ２ブロック群における値と比較することで行う。例えば、一つのブロックＡについて着目すると、ブロックＡがＮｏ２ブロック群において正しい位置に選択されたか否かを評価するには、まず、Ｎｏ１ブロック群におけるブロックＡと他のブロックＢとの距離（Ａ１−Ｂ１）を、Ｎｏ２ブロック群におけるブロックＡとブロックＢとの距離（Ａ２−Ｂ２）と比較し、同様に、ブロックＡ−ブロックＣ、ブロックＡ−ブロックＤについても距離を比較する。
【００９３】
そして、３つともＮｏ１ブロック群とＮｏ２ブロック群とにおける距離が等しければ距離のチェックはＯＫとし、そうでなければ、次のブロックＥ，Ｇ，…と順次比較するブロックを変えてゆき、先に３個ＯＫになれば距離のチェックはＯＫ、先に３個ＮＧになれば距離のチェックはＮＧとする。
【００９４】
このように、ブロックＡの３点（Ｂ，Ｃ，Ｄ）に対する距離のチェックがＯＫになった場合、次に、ブロックＡ、Ｂ，Ｃの各代表座標で構成される三角形の面積ＡＢＣをＮｏ１ブロック群とＮｏ２ブロック群とについて求め、大きさを比較する。この比較を三角形ＡＣＤについても行い、どちらもＮｏ１ブロック群とＮｏ２ブロック群とで同じ面積であれば、最終的に、ブロックＡの位置はＮｏ２ブロック群において正しく選択されたと判定する。
【００９５】
この際、距離のチェックは、ピタゴラスの定理による二乗値のまま比較し、また、三角形の面積のチェックは、ベクトルの外積を計算して三角形の面積の２倍の値（平行四辺形の面積）で比較することにより、計算効率を高める。
【００９６】
以上の配列パターンの評価により適正でないブロックを排除した後、ステップS52へ進み、適正でないブロックを排除した後のＮｏ２ブロック数が所定の数か否かを調べる。そして、計算精度を維持するのに必要な所定の数に達していないときには、上記ステップS52からステップS55へ分岐して前回の角速度成分の計算結果を今回処理の値としてメモリを更新した後、ステップS56へ進み、所定の数のＮｏ２ブロックを確保できたときには、上記ステップS52からステップS53へ進む。
【００９７】
ステップS53では、Ｎｏ２ブロックの撮像面座標を、カメラ固定のＸ，Ｙ，Ｚ直交座標である実空間座標に直す。レンズの焦点距離ｆ、メインカメラ軸Ｚｍとサブカメラ軸Ｚｓとの基線長ＬS（Ｓｓ）、ステレオ画像のズレ量Ｘｒとすると、測距対象点Ｍまでの距離Ｚは、三角測量によって以下の(4)式で与えられるため、この関係に基づいて撮像面座標を実空間座標に変換する。
【００９８】
Ｚ＝ＬS（ＳS）・ｆ／Ｘｒ …(4)
続くステップS54では、非線形型最小二乗法によってＮｏ１ブロック群とＮｏ２ブロック群との間の位置の差分に相当する角速度（ロール・ピッチ・ヨー）を求める。すなわち、前回の回転・並進の速度成分を初期値として撮像素子上の動きに換算してＮｏ１ブロックとＮｏ２ブロックとの位置の差を求め、さらに、線形近似を行って３軸（Ｘ，Ｙ，Ｚ軸）の回転成分毎に偏微分し、適当な偏角を与えてロール・ピッチ・ヨー毎に撮像素子上のＮｏ１ブロックとの位置の差の変化量（ｄｘ，ｄｙ）を求める。ロール・ピッチ・ヨーの各成分は、この変化量（ｄｘ，ｄｙ）の極限として与えられる。
【００９９】
従って、上記変化量（ｄｘ，ｄｙ）を求める処理をＮｏ１・Ｎｏ２ブロック全てについて行い、最小二乗法で回転成分を求める。この最小二乗法では、３元１次方程式を解くことになるが、係数行列についての転置行列が等しく、対角要素が他の要素に比べて十分に大きい値であるため（正定値対称）、コレスキー法による数値計算を適用することができ、Ｎｏ１ブロックに対してのロール・ピッチ・ヨーの各々の値を求めることができる。
【０１００】
実際には、上記手法による解は、非線形のため真値にはならない。従って、得られた値を初期値にして再び最小二乗法を繰り返す非線形型最小二乗法によって真値に近づけるわけであるが、真値を求める際には、上記変化量（ｄｘ，ｄｙ）の収束具合（理想的には０）を監視して決定すべき処置が必要である。すなわち、限定した範囲の角速度成分を適度な精度で得る場合には、繰り返し計算回数を所定の回数（例えば、５回）で打ち切り、処理に要する時間を短縮することができる。
【０１０１】
上記ステップS54の処理によってカメラ固定のＸＹＺ直交座標系における角速度成分を求めると、次に、ステップS56へ進み、ロール成分ｒｌ、ピッチ成分ｐｔ、及び、ヨー成分ｙｗを、回転行列Ｒｏｔの要素に展開してルーチンを抜ける。この回転行列Ｒｏｔは、ロール、ピッチ、ヨーの各軸毎の回転行列の積であり、例えば、以下の(5)〜(13)式で示すように、３×３の正方行列の要素として展開できる。
Ｒｏｔ[0][0]＝(ｃｙ・ｃｒ)−(ｓｙ・ａｎｓ) …(5)
Ｒｏｔ[1][0]＝−(ｃｐ・ｓｒ) …(6)
Ｒｏｔ[2][0]＝(ｓｙ・ｃｒ)＋(ｃｙ・ａｎｓ) …(7)
Ｒｏｔ[0][1]＝(ｃｙ・ｓｒ)＋(ｓｙ・ａｎｓ') …(8)
Ｒｏｔ[1][1]＝(ｃｐ・ｃｒ) …(9)
Ｒｏｔ[2][1]＝(ｓｙ・ｓｒ)−(ｃｙ・ａｎｓ') …(10)
Ｒｏｔ[0][2]＝−(ｓｙ・ｃｐ) …(11)
Ｒｏｔ[1][2]＝ｓｐ …(12)
Ｒｏｔ[2][2]＝(ｃｙ・ｃｐ) …(13)
但し、ｃｙ＝ｃｏｓ(ｙｗ) ｃｒ＝ｃｏｓ(ｒｌ)
ｃｐ＝ｃｏｓ(ｐｔ) ｓｙ＝ｓｉｎ(ｙｗ)
ｓｒ＝ｓｉｎ(ｒｌ) ｓｐ＝ｓｉｎ(ｐｔ)
ａｎｓ＝ｓｐ・ｓｒ ａｎｓ'＝ｓｐ・ｃｒ
【０１０２】
次に、回転速度成分と並進速度成分とを含む下方画像の動きから純並進速度成分を求める処理について説明する。この処理は、図１５の並進演算処理ルーチンによって行われ、まず、ステップS60で、計算精度を維持するに必要な所定のサンプル数だけＮｏ２ブロック群を取得できたか否かを調べ、所定のサンプル数を取得できたときには、ステップS60からステップS61へ進み、所定のサンプル数を取得できないとき、ステップS60からステップS71へ進んで前回の並進速度成分の計算結果を今回処理の値としてメモリを更新し、ルーチンを抜ける。
【０１０３】
ステップS61では、Ｎｏ２ブロックの平均距離画像値を評価する。この評価は、Ｎｏ１ブロックとＮｏ２ブロックとで距離画像値が閾値以上に異なっているブロックを排除するためのものであり、該当するブロックがあるときには、そのブロックを除いてＮｏ２ブロック群全てを評価したか否かを調べるステップS67へジャンプし、このステップS67からステップS61へ戻るループによってＮｏ１ブロック群及びＮｏ２ブロック群全体から距離画像値が閾値以上に異なっているブロックを排除する。
【０１０４】
その後、上記ステップS61によるＮｏ２ブロックの平均距離画像値の評価を経てステップS62へ進み、Ｎｏ１・Ｎｏ２ブロック群の個々のサンプルを、レンズ歪み分を補正した撮像面座標値に修正し、測距精度のバラツキが少ない新たなＮｏ１・Ｎｏ２ブロック群に更新する。尚、ステップS61→ステップS67→ステップS61のループで排除したブロック数が多く、計算精度を維持するに必要なサンプル数を確保できないときには、前回の並進速度成分の計算結果を今回処理の値として並進演算処理を終了する。
【０１０５】
続くステップS63では、Ｎｏ１・Ｎｏ２ブロックの撮像面座標値と、それぞれの距離画像値とから実空間の座標（カメラ固定のＸＹＺ直交座標）値を求め、ステップS64で、Ｎｏ２ブロックから求めた実空間座標値を、最新の回転行列で回転成分を差し引いた純並進成分の実空間座標値に変換し、ステップS65へ進んで、画像の動き（移動ベクトル）を実空間座標値の差分で求める。
【０１０６】
すなわち、遠方画像の撮像面と下方画像の撮像面とは互いに直交関係にあることから、回転移動と並進移動とを含んだＮｏ１・Ｎｏ２ブロック間の移動ベクトル量に、先に求めた回転行列Ｒｏｔ（Ｎｏ２→Ｎｏ１ブロックへの回転行列）を掛け合わせることにより（相対的に回転させることにより）、回転成分の動きを実距離のクラスで除去することができる。
【０１０７】
次いで、ステップS66へ進み、実空間移動量を新たに記録して測距したブロック数を勘定すると、ステップS67で、Ｎｏ２ブロック群の全てを評価したか否かを調べる。そして、全部の評価が終わっていないときには、ステップS61へ戻り、全部の評価が終わったとき、ステップS68へ進み、区間推定等の統計処理によってオプティカルフロー全体に対して方向や大きさが異常である移動ベクトル（特異ベクトル）を排除するフィルタ処理を行い、特異ベクトルを排除した後の平均移動量を算出する。
【０１０８】
その後、ステップS69で、特異ベクトルの除去結果を調べ、特異ベクトルの数が多く、航法計算の精度を維持するのに必要なデータ数が確保できなくなったときには、前述のステップS71で前回の並進速度成分の計算結果を今回処理の値としてメモリを更新してルーチンを抜ける。一方、特異ベクトルが殆ど無いか、あるいは航法計算の精度を損なわない程度の数であるときには、上記ステップS69からステップS70へ進んで、成分（Ｘｌ,Ｙｌ,Ｚｌ）の３次元並進速度｜Ｓ｜を求め、この新しい並進速度でメモリ値を更新してルーチンを抜ける。
【０１０９】
以上の並進演算処理に続いて航法軌跡を求める図１６の航法演算処理が実行される。この航法演算処理では、まず、ステップS80で原点（測距開始地点）から見た移動体の姿勢状態を求める。原点から見た移動体の姿勢状態は、測距開始地点で空間に固定したＸＹＺ直交座標系から見た回転行列Ｒｏｔｓによって示される。このため、まず、測距開始地点で回転行列を単位行列化して初期化することで撮像面に直交する平面を実空間を表す基準面とし、この初期化時点の行列にフレーム間の回転行列（カメラ固定のＸＹＺ直交座標系から見た回転行列）Ｒｏｔを右から掛け込んだ行列を原点から見た移動体の姿勢を表す回転行列Ｒｏｔｓとする。
【０１１０】
すなわち、原点から見た姿勢を表す回転行列Ｒｏｔｓは、カメラ固定の座標系から見たフレーム毎の回転行列Ｒｏｔを時系列的に積算した正方行列であり、以下の(14)式に示すように、前回までの原点から見た姿勢状態を表す回転行列Ｒｏｔｓに今回の回転演算処理で求めたフレーム間の回転行列Ｒｏｔを右から積算した正方行列で今回処理における原点から見た姿勢を表す回転行列Ｒｏｔｓとして更新することにより、原点から見た移動体の最新の姿勢を表すことができる。
Ｒｏｔｓ＝Ｒｏｔｓ・Ｒｏｔ …(14)
【０１１１】
尚、上記回転行列Ｒｏｔｓを用いることにより、以下の(15)〜(17)式によって原点から見た角度成分Ｒｏｌｌ（ロール），Ｐｉｃｔｈ（ピッチ），Ｙｏｗ（ヨー）を求めることができる。
Ｒｏｌｌ ＝ａｔａｎ(Ｒｏｔｓ[0][1]／Ｒｏｔｓ[0][0]) …(15)
Ｐｉｔｃｈ＝ａｔａｎ(Ｒｏｔｓ[1][2]／Ｒｏｔｓ[2][2]) …(16)
Ｙｏｗ ＝ａｔａｎ(Ｒｏｔｓ[2][0]／Ｒｏｔｓ[2][2]) …(17)
【０１１２】
次に、ステップS81へ進み、フレーム間の並進速度成分を原点から見た並進速度成分に変換すると、ステップS82で、原点から見た速度成分をフレーム毎に累積し、イニシャル地点からの移動体の実空間座標を求める。
【０１１３】
原点から見た並進速度Ｓｏは、原点から見た姿勢を表す回転行列Ｒｏｔｓとフレーム間の３次元並進速度Ｓとの積を、以下の(18)式に示すように、前回までの原点から見た並進速度Ｓｏに累積することで求めることができ、累積したベクトル量に所定の距離換算値を掛け合わせることで、航法軌跡を得ることができ、原点からの移動体の移動量を知ることができる。
Ｓｏ＝Ｓｏ＋(Ｒｏｔｓ)・Ｓ …(18)
【０１１４】
この場合、地上の適当な地点にランドマークを設置しておき、このランドマークをパターン認識等によって識別し、測距開始地点として原点をイニシャルすることで、上記航法軌跡から自己の相対位置を認識することができる。さらには、ＧＰＳ等の衛星測位システムからの測位情報や既知の地図情報によって地上の適当な地点を原点としてイニシャルすることで、航法軌跡から自己の絶対位置を認識することができる。
【０１１５】
【発明の効果】
以上説明したように本発明によれば、高速なハードウエア回路によってリアルタイムでオプティカルフローを検出することができ、移動体に搭載可能な小型の装置で撮像画像からオンラインでオプティカルフローを検出して移動体の自己位置を認識することができる等優れた効果が得られる。
【図面の簡単な説明】
【図１】自己位置認識システムの全体構成図
【図２】カメラアセンブリの構成図
【図３】ステレオ処理部のブロック図
【図４】オプティカルフロー処理部のブロック図
【図５】ヒストグラム処理回路の構成図
【図６】自己加算型階調・度数モジュールの回路構成図
【図７】データ転送バッファの構成図
【図８】演算処理回路の構成図
【図９】ステレオ処理の説明図
【図１０】ヒストグラム処理の説明図
【図１１】自己加算型階調・度数モジュールのタイムチャート
【図１２】オプティカルフロー処理の説明図
【図１３】Ｎｏ１ブロック群取得処理ルーチンのフローチャート
【図１４】回転演算処理ルーチンのフローチャート
【図１５】並進演算処理ルーチンのフローチャート
【図１６】航法演算処理ルーチンのフローチャート
【符号の説明】
１０…カメラアセンブリ
２０…自己位置認識装置
３０…ステレオ処理部
３９…ステレオマッチング回路
６０…オプティカルフロー処理部
７０…ヒストグラム処理回路
８１…データ転送バッファ
８５…演算処理回路
９０…航法処理部
ＳＹＳ１…画像処理プロセッサ系
ＳＹＳ２…航法処理プロセッサ系

Claims (2)

1. 異なるタイミングで撮像した２枚の画像の一方を基準画像として他方を比較画像とし、互いの対応位置を探索して画像間のオプティカルフローを求める画像のオプティカルフロー検出装置であって、
   ステレオカメラを有する撮像部と、
   上記ステレオカメラで撮像した２枚の画像における対応位置を探索して対象物までの距離に応じて生じる画素ズレ量を求め、この画素ズレ量から得られる対象物までの遠近情報を数値化した距離画像を生成する距離画像生成部と、
   上記距離画像の階調データ及び度数を、それぞれラッチに保持し、新たに入力された階調データと保持した階調データとを比較する比較器からの一致出力により、保持した階調データの度数を積算してヒストグラムデータを生成するヒストグラム生成部と、
   上記ヒストグラム生成部で生成したヒストグラムデータを参照し、基準画像の中から特徴的な絵柄の領域を第１ブロックとして複数箇所抽出するとともに、比較画像の中から上記第１ブロックに対応する領域を第２ブロックとして探索するための探索範囲を設定する画像処理部と、
   上記基準画像の第１ブロックの画素毎の輝度データと上記比較画像の第２ブロックの画素毎の輝度データとを、それぞれ所定バイト数のデータ系列に並び変え、各データ系列をリング状に転送するデータ転送バッファと、
   上記転送バッファから転送された２系統のデータ間の差分の絶対値を複数の演算器で並列に演算した後、各演算器の出力を複数段の加算器で直列に加算してシティブロック距離を計算し、このシティブロック距離が最小値となるアドレスを、上記基準画像の第１ブロックに対する上記比較画像の対応位置として出力する演算処理回路とを備えたことを特徴とする画像のオプティカルフロー検出装置。
2. 移動体に搭載されて遠方風景を撮像する２台１組のステレオカメラ及び下方風景を撮像する２台１組のステレオカメラを有する撮像部と、
   上記ステレオカメラで撮像した２枚の画像における対応位置を探索して対象物までの距離に応じて生じる画素ズレ量を求め、この画素ズレ量から得られる対象物までの遠近情報を数値化した距離画像を生成する距離画像生成部と、
   上記距離画像の階調データ及び度数を、それぞれラッチに保持し、新たに入力された階調データと保持した階調データとを比較する比較器からの一致出力により、保持した階調データの度数を積算してヒストグラムデータを生成するヒストグラム生成部と、
   上記距離画像から上記ヒストグラム生成部で生成したヒストグラムデータを参照し、撮像画像の中から航法演算に適した特徴的な絵柄の領域を第１ブロックとして複数箇所抽出するとともに、この第１ブロックを抽出した画像とは撮像タイミングが異なる画像の中から上記第１ブロックに対応する領域を第２ブロックとして探索するための探索範囲を設定する画像処理部と、
   上記第１ブロックの画素毎の輝度データと上記第２のブロックの探索範囲の画素毎の輝度データとを、それぞれ所定バイト数のデータ系列に並び変え、各データ系列をリング状に転送するデータ転送バッファと、この転送バッファから転送された２系統のデータ間の差分の絶対値を複数の演算器で並列に演算した後、各演算器の出力を複数段の加算器で直列に加算してシティブロック距離を計算し、このシティブロック距離が最小値となるアドレスを出力する演算処理回路とを有するオプティカルフロー処理部と、
   遠方風景の撮像画像に対する上記オプティカルフロー処理部からの出力と上記距離画像とに基づいて上記第１ブロックから上記第２ブロックへのオプティカルフローを求めて移動体のフレーム間の回転速度成分を算出するとともに、下方風景の撮像画像に対する上記オプティカルフロー処理部からの出力と上記距離画像とに基づいて上記第１ブロックから上記第２ブロックへのオプティカルフローを求めて移動体のフレーム間の速度成分を算出し、この速度成分から上記回転速度成分を除去したフレーム間の並進速度成分を測距開始地点から見た並進速度成分に変換し、この変換した並進速度成分を累積して３次元空間における航法軌跡を求めることにより、移動体の自己位置を認識する航法処理部とを備えたことを特徴とする移動体の自己位置認識システム。

Images