JP3833786B2 - 移動体の３次元自己位置認識装置 - Google Patents

Description

【０００１】
【発明の属する技術分野】
本発明は、自律航法によって移動する移動体の３次元自己位置認識装置に関する。
【０００２】
【従来の技術】
従来より、無人ロボット、自律走行作業車、無人ヘリコプター等の自律的に移動する移動体に関し、移動制御、経路検出、航路検出、ロケーション検出等の各種技術が開発されており、これらの中でも、自己位置認識は重要な技術の一つである。
【０００３】
この自己位置認識の技術としては、例えば、自律走行作業車等の地上を自律的に走行する移動体では、振動ジャイロや光ジャイロによって２次元の角速度を検出するとともに、対地速度を測定するセンサによって並進速度を検出し、基準位置からの移動量を演算して自己位置を測定する技術があり、また、無人ヘリコプター等の飛行移動体では、慣性航法装置により、重力加速度を検出して飛行体の加速度を検出し、この加速度を積分することで移動量を知る技術がある。
【０００４】
さらに、近年では、ＧＰＳ衛星等のように、少なくとも３つの人工衛星の電波を受信し、２台のＧＰＳ受信機で受ける電波の位相差あるいは単独で符号通信内容（衛星の位置と電波の発射更新時間）を解析し、自己位置を割り出す技術も採用されており、本出願人は、先に、特願平８−２４９０６１号公報において、衛星利用の測位データにより基準位置からの走行履歴に基づく測位データを補正し、制御安定性を向上した自律走行車を提案している。
【０００５】
【発明が解決しようとする課題】
しかしながら、無人ヘリコプター等のような飛行体では、３次元空間における自己の回転や並進の速度を知る必要があるばかりでなく、低空飛行の際には正確な対地高度や木目細かな航法情報を得る必要がある。
【０００６】
このような場合、従来の装置では、測位精度低下の原因となるドリフトの影響によって精度が不足するばかりでなく、地形が複雑であると、正確な対地高度を得ることは困難である。すなわち、従来の技術では、３次元空間における周囲環境を細かく分析し、航法情報に反映させるという点で不十分である。
【０００７】
本発明は上記事情に鑑みてなされたもので、３次元空間における周囲環境を分析して航法情報に反映させ、高精度で緻密な航法情報を得ることのできる移動体の３次元自己位置認識装置を提供することを目的としている。
【０００８】
【課題を解決するための手段】
請求項１記載の発明は、移動体に搭載されるステレオカメラにより、一定時間毎に遠方風景と下方風景とを撮像する手段と、遠方風景の撮像画像と下方風景の撮像画像とに対し、それぞれの撮像画像の中から特徴的な絵柄の領域を第１のブロックとして複数箇所抽出し、次のタイミングで撮像した画像の中から第１のブロックと同じ絵柄を有する領域を第２のブロックとして探索する手段と、遠方風景の撮像画像における第１のブロックから第２のブロックへの画像の動きを、遠方風景の撮像画像をステレオ画像処理して求めた距離画像に基づいて撮像面座標を実距離に換算した実空間座標での移動量として求め、該移動量とフレーム間の時間経過とから、移動体のフレーム間のカメラ固定の座標系における回転速度成分を求める手段と、下方風景の撮像画像における第１のブロックから第２のブロックへの画像の動きを、下方風景の撮像画像をステレオ画像処理して求めた距離画像に基づいて撮像面座標を実距離に換算した実空間座標での移動量として求め、該移動量とフレーム間の時間経過とから、移動体のフレーム間のカメラ固定の座標系における速度成分を求め、この速度成分から上記回転速度成分を除去して移動体のフレーム間のカメラ固定の座標系における並進速度成分を求める手段と、上記フレーム間のカメラ固定の座標系における並進速度成分を測距開始地点で空間に固定した座標系における並進速度成分に変換し、この変換した並進速度成分を累積して３次元空間における航法軌跡を求めることにより、移動体の自己位置を認識する手段とを備えたことを特徴とする。
【００１０】
請求項２記載の発明は、請求項１記載の発明において、上記フレーム間のカメラ固定の座標系における並進速度成分を、測距開始地点で空間に固定した座標系の原点から見た移動体の最新の姿勢を表す回転成分で補正し、測距開始地点で空間に固定した座標系における並進速度成分に変換することを特徴とする。
【００１１】
請求項３記載の発明は、請求項２記載の発明において、測距開始地点で空間に固定した座標系における移動体の姿勢を表す行列を単位行列化して初期化した後、今回処理時のフレーム間のカメラ固定の座標系における回転速度成分を表す行列を測距開始地点で空間に固定した座標系の原点から見た前回までの移動体の姿勢を表す行列に積算し、上記移動体の最新の姿勢を表す回転成分とすることを特徴とする。
【００１２】
請求項４記載の発明は、請求項１記載の発明において、第１のブロックを抽出する際、撮像画面を所定範囲で分割した複数の探索エリアを設定し、各探索エリア毎に第１のブロックを最大１個抽出することを特徴とする。
【００１３】
請求項５記載の発明は、請求項１記載の発明において、第１のブロックをｍ個抽出する際、撮像画面をｎ個の領域に分割し、各領域で抽出する第１のブロックを最大ｍ／ｎ個とすることを特徴とする。
【００１４】
請求項６記載の発明は、請求項１記載の発明において、第１のブロックを遠方風景の撮像画像から抽出する際、上記距離画像の対応する領域の距離画像値が小さく、測距点から十分に遠方にある小領域を、第１のブロックの候補とすることを特徴とする。
【００１５】
請求項７記載の発明は、請求項１記載の発明において、第１のブロックを下方風景の撮像画像から抽出する際、上記距離画像の対応する領域の距離画像値のバラツキが少ない平面的な小領域を、第１のブロックの候補とすることを特徴とする。
【００１６】
請求項８記載の発明は、請求項１記載の発明において、第１のブロックを下方風景の撮像画像から抽出する際、画面の中央ラインから進行方向側に所定量オフセットさせたラインを探索開始ラインとして設定し、この探索開始ラインを上記並進速度成分によって可変することを特徴とする。
【００１７】
請求項９記載の発明は、請求項１記載の発明において、第１のブロックを抽出する際、対象とする領域の画像品質を、上記距離画像の対応する領域の距離画像値を用いて作成したヒストグラムによって評価することを特徴とする。
【００１８】
請求項１０記載の発明は、請求項９記載の発明において、上記ヒストグラムを、抽出ピクセルが所定の閾値以上の度数をカウントして作成することを特徴とする。
【００１９】
請求項１１記載の発明は、請求項９記載の発明において、最大ヒストグラムを記録する距離画像値に対して所定範囲にあるヒストグラム値の総度数が所定の比率以上のとき、対象とする領域の画像品質が良好であると判定することを特徴とする。
【００２０】
請求項１２記載の発明は、請求項１記載の発明において、第２のブロックを探索する際、第１のブロックを囲む所定の探索エリアを設定し、この探索エリアから第２のブロックを探索することを特徴とする。
【００２１】
請求項１３記載の発明は、請求項１記載の発明において、第２のブロックを探索する際、前回の回転速度成分あるいは並進速度成分から次の画面での第１のブロックの移動先の撮像面座標を予測し、予測した撮像面座標を囲む所定の探索エリアから第２のブロックを探索することを特徴とする。
【００２２】
請求項１４記載の発明は、請求項１記載の発明において、第２のブロックを探索する際、第１のブロックを囲む所定の探索エリアを設定し、この探索エリアと第１のブロックとの間でシティブロック距離が最小となる領域を第２のブロックの候補とすることを特徴とする。
【００２３】
請求項１５記載の発明は、請求項１記載の発明において、第２のブロックを探索する際、前回の回転速度成分あるいは並進速度成分から次の画面での第１のブロックの移動先の撮像面座標を予測し、予測した撮像面座標を囲む所定の探索エリアと第１のブロックとの間でシティブロック距離が最小となる領域を第２のブロックの候補とすることを特徴とする。
【００２４】
請求項１６記載の発明は、請求項１記載の発明において、第１のブロックと第２のブロックとの対を所定数取得できないとき、前回処理で求めた回転速度成分及び並進速度成分を今回処理の回転速度成分及び並進速度成分として更新することを特徴とする。
【００２５】
請求項１７記載の発明は、請求項１記載の発明において、複数の第１のブロックの配列パターンと複数の第２のブロックの配列パターンとの相違を調べて配置が異なるブロックを排除した後、第１のブロックと第２のブロックとの対を所定数確保できたとき、上記フレーム毎の移動体の回転速度成分を求める処理へ移行し、第１のブロックと第２のブロックとの対を所定数確保できないときには、前回処理で求めた回転速度成分を今回処理の回転速度成分として更新することを特徴とする。
【００２６】
請求項１８記載の発明は、請求項１７記載の発明において、上記配列パターンの相違を、各ブロック間の距離と各ブロックによって構成される三角形の面積とによって調べることを特徴とする。
【００２７】
請求項１９記載の発明は、請求項１記載の発明において、上記測距開始地点の位置を、測位装置からの情報あるいは既知の地図情報によって初期化し、上記航法軌跡から移動体の絶対位置を認識することを特徴とする。
【００２８】
請求項２０記載の発明は、請求項１記載の発明において、上記ステレオカメラを、遠方風景撮像用のステレオカメラと下方風景撮像用のステレオカメラとから構成し、互いのステレオカメラの撮像面の方向が直交するよう配置することを特徴とする。
【００２９】
請求項２１記載の発明は、請求項２０記載の発明において、上記遠方風景撮像用のステレオカメラと上記下方風景撮像用のステレオカメラとを、互いの基準カメラ同士が近接した状態に配置することを特徴とする。
【００３０】
すなわち、本発明による移動体の自己位置認識装置では、一定時間毎に遠方風景と下方風景とを撮像し、それぞれの撮像画像の中から特徴的な絵柄の領域を第１のブロックとして複数箇所抽出し、次のタイミングで撮像した画像の中から第１のブロックと同じ絵柄を有する領域を第２のブロックとして探索すると、遠方風景の撮像画像における第１のブロックから第２のブロックへの画像の動きを、遠方風景の撮像画像をステレオ画像処理して求めた距離画像に基づいて撮像面座標を実距離に換算した実空間座標での移動量として求め、該移動量とフレーム間の時間経過とから、移動体のフレーム間のカメラ固定の座標系における回転速度成分を求め、また、下方風景の撮像画像における第１のブロックから第２のブロックへの画像の動きを、下方風景の撮像画像をステレオ画像処理して求めた距離画像に基づいて撮像面座標を実距離に換算した実空間座標での移動量として求め、該移動量とフレーム間の時間経過とから、移動体のフレーム間のカメラ固定の座標系における速度成分を求め、この速度成分から上記回転速度成分を除去して移動体のフレーム間のカメラ固定の座標系における並進速度成分を求める。そして、このフレーム間のカメラ固定の座標系における並進速度成分を測距開始地点で空間に固定した座標系における並進速度成分に変換し、この変換した並進速度成分を累積して３次元空間における航法軌跡を求め、移動体の自己位置を認識する。
【００３１】
この場合、測距開始地点の位置を測位装置からの情報あるいは既知の地図情報によって初期化することで移動体の絶対位置を認識することができ、また、フレーム間のカメラ固定の座標系における並進速度成分は、測距開始地点で空間に固定した座標系の原点から見た移動体の最新の姿勢を表す回転成分で補正して測距開始地点で空間に固定した座標系における並進速度成分に変換することができる。
【００３２】
測距開始地点で空間に固定した座標系の原点から見た移動体の最新の姿勢を表す回転成分は、測距開始地点で空間に固定した座標系における移動体の姿勢を表す行列を単位行列化して初期化した後、今回処理時のフレーム間のカメラ固定の座標系における回転速度成分を表す行列を測距開始地点で空間に固定した座標系の原点から見た前回までの移動体の姿勢を表す行列に積算することで得られる。
【００３３】
また、第１のブロックを抽出する際には、撮像画面を所定範囲で分割した複数の探索エリアを設定し、各探索エリア毎に第１のブロックを最大１個抽出する、あるいは、第１のブロックをｍ個抽出する際、撮像画面をｎ個の領域に分割し、各領域で抽出する第１のブロックを最大ｍ／ｎ個とすることが望ましく、撮像画面の特定の領域に抽出箇所が集中せずに画面全体に分散した特徴的な絵柄を抽出することができる。
【００３４】
また、第１のブロックを下方風景の撮像画像から抽出する際には、画面の中央ラインから進行方向側に所定量オフセットさせたラインを探索開始ラインとして設定し、この探索開始ラインを上記並進速度成分によって可変することが望ましく、下方画像の動きを考慮して前・後進速度に応じて撮像面を有効に利用することができる。
【００３５】
さらに、第２のブロックを探索する際には、第１のブロックを囲む所定の探索エリアを設定し、この探索エリアから第２のブロックを探索することで、限定した領域から確実な画像マッチングを行うことができ、特に、前回の回転速度成分あるいは並進速度成分から次の画面での第１のブロックの移動先の撮像面座標を予測し、予測した撮像面座標を囲む所定の探索エリアから第２のブロックを探索することが望ましい。
【００３６】
また、第１のブロックと第２のブロックとの対を所定数取得できないときには、前回処理で求めた回転速度成分及び並進速度成分を今回処理の回転速度成分及び並進速度成分として更新する、あるいは、複数の第１のブロックの配列パターンと複数の第２のブロックの配列パターンとの相違を調べて配置が異なるブロックを排除した後、第１のブロックと第２のブロックとの対を所定数確保できたとき、フレーム毎の移動体の回転速度成分を求める処理へ移行し、第１のブロックと第２のブロックとの対を所定数確保できないときには、前回処理で求めた回転速度成分を今回処理の回転速度成分として更新することが望ましく、誤った情報による誤差を排除することができる。この配列パターンの相違は、各ブロック間の距離と各ブロックによって構成される三角形の面積とによって調べることができる。
【００３７】
一方、距離画像を用いる場合には、第１のブロックを遠方風景の撮像画像から抽出する際、距離画像の対応する領域の距離画像値が小さく、測距点から十分に遠方にある小領域を第１のブロックの候補とし、第１のブロックを下方風景の撮像画像から抽出する際、上記距離画像の対応する領域の距離画像値のバラツキが少ない平面的な小領域を第１のブロックの候補とすることが望ましく、航法計算に適した特徴的な絵柄を抽出することができる。
【００３８】
また、距離画像を用いて第１のブロックを抽出する際には、対象とする領域の画像品質を、距離画像の対応する領域の距離画像値を用いて作成したヒストグラムによって評価することが望ましく、ヒストグラムを、抽出ピクセルが所定の閾値以上の度数をカウントして作成し、最大ヒストグラムを記録する距離画像値に対して所定範囲にあるヒストグラム値の総度数が所定の比率以上のとき、対象とする領域の画像品質が良好であると判定することが望ましい。
【００３９】
また、第２のブロックを探索する際には、第１のブロックを囲む所定の探索エリアを設定し、この探索エリアと第１のブロックとの間でシティブロック距離が最小となる領域を第２のブロックの候補とすることが望ましく、特に、前回の回転速度成分あるいは並進速度成分から次の画面での第１のブロックの移動先の撮像面座標を予測し、予測した撮像面座標を囲む所定の探索エリアと第１のブロックとの間でシティブロック距離が最小となる領域を第２のブロックの候補とすることが望ましい。
【００４０】
さらに、ステレオカメラを、遠方風景撮像用のステレオカメラと下方風景撮像用のステレオカメラとから構成し、互いのステレオカメラの撮像面の方向が直交するよう配置することが望ましく、遠方風景撮像用のステレオカメラと下方風景撮像用のステレオカメラとを、互いの基準カメラ同士が近接した状態に配置することで、移動体の回転運動の検出中心と並進運動の検出中心とを略一致させることができる。
【００４１】
【発明の実施の形態】
以下、図面を参照して本発明の実施の形態を説明する。図面は本発明の実施の一形態に係わり、図１は自己位置認識装置の機能構成図、図２は３次元空間計測用カメラアセンブリの構成図、図３は自己位置認識装置の回路ブロック図、図４は並列処理のタイムチャート、図５はＳＹＳ１初期化処理ルーチンのフローチャート、図６はＳＹＳ２初期化処理ルーチンのフローチャート、図７はＮｏ１ブロック群取得処理ルーチンのフローチャート、図８はＮｏ２ブロック群取得処理ルーチンのフローチャート、図９は回転演算処理ルーチンのフローチャート、図１０は並進演算処理ルーチンのフローチャート、図１１は航法演算処理ルーチンのフローチャート、図１２は遠方のＮｏ１ブロックの取得パターンを示す説明図、図１３は画像評価におけるヒストグラム分布の好ましくない例を示す説明図、図１４は画像評価におけるヒストグラム分布の好ましい例を示す説明図、図１５は下方のＮｏ１ブロックの取得パターンを示す説明図、図１６は探索エリアの設定を示す説明図、図１７は遠方画像の特徴を示す説明図、図１８は遠方画像に探索エリアを設定した例を示す説明図、図１９は下方の撮像面上の動きを示す説明図、図２０は下方画像に探索エリアを設定した例を示す説明図、図２１は三角測量による撮像面座標から実距離への換算を示す説明図、図２２は測距開始地点のランドマークを示す説明図、図２３は自律航法による航法軌跡の説明図である。
【００４２】
図１は、３次元空間計測用の複数のカメラからなるカメラアセンブリ１０と、このカメラアセンブリ１０で撮像した画像から距離情報を取得するステレオ画像処理装置２０と、画像情報から３次元空間における移動体の自己位置を認識し、航法データを外部のターゲットコントロールシステム１００に出力する自己位置認識装置３０等から構成される移動体の３次元自己位置認識装置であり、本形態においては、例えば、農薬散布等に使用される無人ヘリコプターに搭載される自律航法装置への適用例を示し、ハンドシェイクまたはタイムスケジュールによる並列処理によってデータ処理が行われる。
【００４３】
上記カメラアセンブリ１０は、移動体に搭載されて周囲環境を一定時間毎に撮像し、周囲環境の変化を１画面単位で捕らえるとともに、水平ズレに応じた距離情報を取得するための測距センサ的役割を果たすものであり、図２に示すように、架台１１に、移動体の回転速度成分を演算するために必要な遠方風景を撮像するための１組の遠方用ステレオカメラ１２と、移動体の並進速度成分を演算するのに必要な下方風景（地表面）を撮像するための１組の下方用ステレオカメラ１３とが設置されている。
【００４４】
上記遠方用ステレオカメラ１２は、メインカメラ（基準カメラ）１２ａと、このメインカメラ１２ａと同じ仕様のサブカメラ１２ｂとが基線長ＬSの関係で、且つ、互いの撮像面垂直軸が平行となるよう配置されており、また、上記下方用ステレオカメラ１３は、メインカメラ（基準カメラ）１３ａと、このメインカメラ１３ａと同じ仕様のサブカメラ１３ｂとが基線長ＳSの関係で、且つ、互いの撮像面垂直軸が平行となるように配置されている。
【００４５】
本発明による自己位置認識装置では、遠方風景の撮像画像及び下方風景の撮像画像のそれぞれについて、撮像時間の異なる画像間の動きから移動体の運動を検出するようにしており、遠方画像の動き、下方画像の動きを、それぞれの距離画像に基づいて実空間での移動量に換算し、図２３に示すように、下方画像の動きによる速度成分から遠方画像の動きによる回転速度成分を除去して純並進速度成分を求めた後、測距開始地点（スタート地点）から見た並進速度成分に変換して累積することにより、３次元空間における航法軌跡を求めて移動体の自己位置を認識する。
【００４６】
この場合、遠方用ステレオカメラ１２によって移動体の回転速度を検出し、下方用ステレオカメラ１３によって移動体の回転・並進速度を検出するためには、遠方用ステレオカメラ１２の撮像面垂直軸と下方用ステレオカメラ１３の撮像面垂直軸とが互いに直交し、且つ、遠方のメインカメラ軸と下方のメインカメラ軸とが同一平面で互いの基準点が一致していることが理想的である。
【００４７】
しかしながら、２つのメインカメラ１２ａ，１３ａの基準点を同じにすることは現実的には困難であるため、遠方用ステレオカメラ１２の撮像面垂直軸と上記下方用ステレオカメラ１３の撮像面垂直軸とが互いに直交するよう配置するとともに、遠方用のメインカメラ１２ａと下方用のメインカメラ１３ａとを寄せた配置とし、一方のカメラの３軸が他方のカメラの３軸のいずれかと平行関係になるよう配置することで、２組のステレオカメラ１２，１３から得られる遠方画像及び下方画像の動きに対し、１つの実空間座標系での取り扱いを可能とする。
【００４８】
この場合、下方用ステレオカメラ１３に３軸中心を置くが、遠方用ステレオカメラ１２が下方用ステレオカメラ１３の周囲を回転し、両者のオフセット量が生じても、後述するように、遠方画像の性質から、遠方画像の動きには影響せず、下方のカメラ軸と遠方のカメラ軸とが直交関係にあることから、並進と回転を含む速度成分から回転速度成分を除く処理や原点（測距開始地点で空間に固定したＸＹＺ座標系原点）から見た航法計算処理を簡素化し、画像の動きを的確に把握することを可能としている。
【００４９】
尚、上記カメラアセンブリ１０に使用するカメラは、ＥＩＡ準拠の白黒カメラの他、カラーＣＣＤカメラ（３−ＣＣＤ方式をも含む）、赤外線カメラ、暗視カメラ等のエリア・スキャンを行う方式のカメラや、撮像素子からの情報をデジタルで出力する方式のカメラ等が使用可能である。
【００５０】
上記ステレオ画像処理装置２０は、主として、メインカメラ１２ａ（１３ａ）の撮像対象領域と同じパターン（模様）を持つ領域箇所をサブカメラ１２ｂ（１３ｂ）の撮像座標から探して撮像素子から対象物までの距離に応じて生じる画素のズレ（＝視差）を求め、このズレ量から得られる対象物までの遠近情報を数値化した３次元画像情報を取得する（ステレオ画像処理）ものであり、遠方元画像メモリ２１、下方元画像メモリ２２、遠方距離画像メモリ２３、下方距離画像メモリ２４等の処理結果を情報内容別に記録するメモリを備えている。
【００５１】
上記遠方元画像メモリ２１には、遠方用のメインカメラ１２ａで撮像した画像をＡＤ変換及びＬＯＧ変換して数値化した画像データが時系列的に記録され、上記下方元画像メモリ２２には、下方用のメインカメラ１３ａで撮像した画像をＡＤ変換及びＬＯＧ変換して数値化した画像データが時系列的に記録される。
【００５２】
また、上記遠方用距離画像メモリ２３には、遠方用ステレオカメラ１２から得られる２つの画像をステレオ画像処理し、メインカメラ１２ａで撮像した撮像対象箇所または対象物までの距離値を数値化したデータが時系列的に記録され、同様に、上記下方用距離画像メモリ２４には、下方用ステレオカメラ１３から得られる２つの画像をステレオ画像処理し、メインカメラ１３ａで撮像した撮像対象箇所または対象物までの距離値を数値化したデータが時系列的に記録される。
【００５３】
尚、上記ステレオ画像処理については、本出願人による特開平５−１１４０９９号公報に詳述されている。
【００５４】
上記自己位置認識装置３０では、所定時間毎に撮像した遠方画像・下方画像の各々に対してパターンの同じ箇所を複数箇所抽出し、それぞれの画像の動きと画面毎の時間経過とから回転・並進速度成分を求めてフレーム間の並進速度成分を原点（測距開始地点で空間に固定したＸＹＺ座標系原点）から見た並進速度成分に変換し、３次元空間で蓄積して航法軌跡を求めることにより、自己位置をリアルタイムで認識する。
【００５５】
上記自己位置認識装置３０の機能は、図１に示すように、初期化処理部３１、Ｎｏ１ブロック群取得処理部３２、Ｎｏ２ブロック群取得処理部３３、回転演算処理部３４、並進演算処理部３５、航法計算処理部３６、通信処理部３７によって示すことができる。
【００５６】
初期化処理部３１では、ハードウエアの初期化、各定数値、各変数値の初期化を行うと共に、以下に述べるＮｏ１ブロック群及びＮｏ２ブロック群の初期データを蓄積する。
【００５７】
Ｎｏ１ブロック群取得処理部３２では、遠方元画像及び下方元画像のそれぞれから、航法演算処理に適正な画像領域の絵柄箇所（以下、Ｎｏ１ブロックと称する）を所定の間隔で切り出す処理を行い、複数のＮｏ１ブロック（Ｎｏ１ブロック群）を、遠方画像、下方画像のそれぞれについて取得する。
【００５８】
この場合、上記Ｎｏ１ブロックは適度に小さい領域とし、画像が多少回転しても平行移動のみで個々のＮｏ１ブロックに対して以下のＮｏ２ブロックを確実にマッチング可能なようにし、また、膨大な情報量の中から測距点までの距離を確実に絞れるようにする。本形態では、Ｎｏ１ブロックの大きさを１２×６ピクセルの小領域とする。
【００５９】
このＮｏ１ブロック群の取得に際しては、１２×６ピクセルの領域に対して出現する距離画像値のヒストグラムを作成し、このヒストグラム情報によって撮像画像の信頼性を評価し、切り出し箇所の良否を判定する。すなわち、距離画像は、左右のカメラが捕らえた画像パターンが同じならば、走査線方向のズレ量に応じた距離画像値が得られるため、この特徴を利用し、同じ距離画像値が周辺に所定の数があれば、対象画像は実在する確かな画像であると判断する。
【００６０】
Ｎｏ２ブロック群取得処理部３３では、所定時間後に撮像した画像に対し、Ｎｏ１ブロックの絵柄と同じ箇所を探索するための探索エリアを設定し、各探索エリア毎にシティブロック距離を計算してマッチング具合を判定し、Ｎｏ１ブロックの絵柄と同じ箇所（以下、Ｎｏ２ブロックと称する）を抽出する。この処理を、遠方画像、下方画像のそれぞれについて行い、複数のＮｏ２ブロック（Ｎｏ２ブロック群）を取得する。
【００６１】
回転演算処理部３４では、遠方画像（遠方元画像）のＮｏ１ブロック群からＮｏ２ブロック群へのオプティカルフロー（Ｎｏ１ブロックのイニシャル座標からＮｏ２ブロックの特定座標までの撮像座標面での動きを表した移動ベクトルの分布）に基づいて実距離成分の差を求め、この実距離成分の差から回転速度成分（ロール・ピッチ・ヨーの角速度）を算出する。
【００６２】
並進演算処理部３５では、下方画像（下方元画像）のＮｏ１ブロック群からＮｏ２ブロック群へのオプティカルフローに基づいて実距離成分の差を求め、この実距離成分の差による速度成分から回転速度成分を除いて３次元方向（Ｘ，Ｙ，Ｚ方向）の純並進速度を求める。
【００６３】
航法計算処理部３６では、上記並進演算処理部３５での演算結果を原点から見た３次元方向の速度に変換し、イニシャル地点から速度成分を累積して原点からの移動量や現在位置を割り出す。また、通信処理部３７では、パラレルあるいはシリアル通信によって航法データを外部のターゲットコントロールシステム１００に出力する。
【００６４】
以上の時系列的なアルゴリズムに対し、実際には、遠方・下方の画像の取り込みは同時に行う必要があり、無人移動システムにおけるナビゲーションとしての実用性を考慮した場合、短い時間周期（例えば、０．１ｓｅｃ毎）で航法データを外部システムに送出することが望ましい。
【００６５】
従って、上記自己位置認識装置３０は、具体的には、図３に示すハードウエア構成となっており、このハードウエア構成によって並列処理による高速処理を実現している。
【００６６】
すなわち、上記自己位置認識装置３０は、マスタースレーブ関係にあるＮｏ１ブロック群取得処理用のマスタープロセッサ（ＳＹＳ１マスター）４０及び通信処理用のスレーブプロセッサ（ＳＹＳ１スレーブ）４１と、同じくマスタースレーブ関係にあるＮｏ２ブロック群取得処理用のマスタープロセッサ（ＳＹＳ２マスター）４２及び回転・並進・航法計算用のスレーブプロセッサ（ＳＹＳ２スレーブ）４３とがデュアルポートＲＡＭ（ＤＰＲＡＭ）４４を介して接続され、さらに、シティブロック計算処理用のマイクロプロセッサ４５、ヒストグラム計算処理用のマイクロプロセッサ４６が接続された基本構成となっている。
【００６７】
上記ＳＹＳ１マスター４０側のバスには、ワークエリア・処理履歴記録用のＤＲＡＭ４７、１２×６ピクセル領域における距離画像値のヒストグラム作成結果が記録されるＤＰＲＡＭ４８、及び、Ｎｏ１ブロック群の探索エリア座標データが記録されるＤＰＲＡＭ４９が接続されている。
【００６８】
一方、上記ＳＹＳ２マスター４２側のバスには、同様にワークエリア・処理履歴記録用のＤＲＡＭ５０、Ｎｏ１ブロック元画像のパターンデータが記録されるＤＰＲＡＭ５１、Ｎｏ２ブロックのシティブロック距離計算結果が記録されるＤＰＲＡＭ５２が接続され、さらに、上記ステレオ画像処理装置２０の各メモリ２１〜２４が接続されている。
【００６９】
そして、上記ヒストグラム計算処理用マイクロプロセッサ４６に、遠方距離画像メモリ２３及び下方距離画像メモリ２４からのデータが入力され、処理結果が上記ＤＰＲＡＭ４８に記録されるとともに、上記シティブロック計算処理用マイクロプロセッサ４５に、遠方元画像メモリ２１及び下方元画像メモリ２２からのデータ、ＤＰＲＡＭ４９，５１からのデータが入力され、処理結果が上記ＤＰＲＡＭ５２に記録される。
【００７０】
尚、図３においては、片方向のデータの流れを矢印の線によって示し、双方向性のデータの流れは矢印の無い線によって示す。
【００７１】
以上のハードウエア構成による処理の流れについて概略すると、図４に示すように、まず、遠方用ステレオカメラ１２で撮像した画像に対し、１２×６ピクセルの距離画像評価を経てＮｏ１ブロック群の取得処理を行い、この処理が終わると、下方用ステレオカメラ１３で撮像した画像に対し、１２×６ピクセルの距離画像評価を経てＮｏ１ブロック群の取得処理が開始される。
【００７２】
同時に、遠方画像に対し、シティブロック距離（ＣＢＫ）評価によるＮｏ２ブロック群のマッチング処理が開始され、この遠方マッチング処理が完了して航法処理が始まると、下方画像に対するＣＢＫ評価を経てＮｏ２ブロック群のマッチング処理が始まる。
【００７３】
航法処理では、回転速度計算から並列速度計算を経て航法軌跡を求める航法計算を行い、航法データが算出されると、通信処理によって外部システムへデータが送信される。そして、この航法データによる自己位置に基づいて、予め設定された絶対座標の航路あるいは地上の目標物に対して設定された航路をとるよう、ヘリコプターの対地高度や飛行方向が精密に制御される。
【００７４】
以下、主として自己位置認識装置３０において実行される自己位置認識に係わる処理について、図５〜図１１のフローチャートに従って説明する。
【００７５】
図５はＳＹＳ１マスター３１におけるＳＹＳ１初期化処理ルーチン、図６はＳＹＳ２マスター３３におけるＳＹＳ２初期化処理ルーチンであり、システム起動時には、Ｎｏ１ブロック群取得処理用のＳＹＳ１マスター３１側のＳＹＳ１初期化処理ルーチンが先に実行される。
【００７６】
図５のＳＹＳ１初期化処理ルーチンでは、ステップS10でＳＹＳ１側のハードウエア各部を初期化し、ステップS11でステレオカメラの仕様定数値を初期化すると、ステップS12で、航法処理変数値や、その他の変数値を初期化する。次いで、ステップS13へ進み、アイドル処理としてＮｏ１ブロック群の取得処理を実行し、Ｎｏ１ブロックを１個取得すると、ステップS14へ進み、アイドル処理終了か否かを判断する。
【００７７】
そして、アイドル処理終了でないときには、上記ステップS14からステップS13へ戻ってＮｏ１ブロックの取得を続行し、アイドル処理終了のとき、ＳＹＳ１初期化処理ルーチンを終了する。このアイドル処理は、航法処理の開始に際してバッファ内にＮｏ１ブロックのデータを蓄積する処理であり、例えば、バッファが３個の場合には、ステップS13→ステップS14→ステップS13のループを３回繰り返す。
【００７８】
以上のＳＹＳ１初期化処理ルーチンでＮｏ１ブロックが取得されると、図６のＳＹＳ２初期化処理ルーチンがスタートし、ステップS20で、同様に、ＳＹＳ２側のハードウエア各部を初期化し、ステップS21でステレオカメラの仕様定数値を初期化すると、ステップS22で、航法処理変数値や、その他の変数値を初期化する。
【００７９】
そして、同様に、ステップS23でのＮｏ２ブロック群の取得処理、ステップS24でのアイドル処理終了判断を経てステップS23→ステップS24→ステップS23の処理ループを所定回数繰り返し、ＳＹＳ２初期化処理ルーチンを終了する。
【００８０】
次に、図７に示すＮｏ１ブロック群の取得処理について説明する。この処理では、まず、ステップS30で、取り込んだ撮像画像に対し、遠方処理をすべきか下方処理をすべきかを選択する。この処理の選択は、撮像画像の処理タイミングが遠方画像が先で下方画像が次であることから、先に取り込んだ画像は遠方画像であるとしてステップS31以降の遠方処理へ進み、次に取り込む画像に対してはステップS33以降の下方処理へ進む。
【００８１】
まず、ステップS31以降の遠方処理では、ステップS31で、遠方画像に対してＮｏ１ブロックを探す走査範囲をイニシャル処理として設定する。この走査範囲は、図１２に示すように、撮像面（ＬＢは遠方元画像、ＬＤは遠方距離画像）の水平走査方向をＷ、垂直走査方向をＴとする全領域Ｗ×Ｔの内、水平走査方向の画面両端の検索しろｉ2と、垂直走査方向の画面両端の検索しろｊ2とを除いた領域ＷＡ×ＴＳＬ（ＷＡ＝Ｗ−２・ｉ2、ＴＳＬ＝Ｔ−２・ｊ2）を対象とする。
【００８２】
そして、水平走査方向の幅ＷＡを均等に１０分割して１０個の探索エリアＷＳ×ＴＳＬを設定し、各探索エリアＷＳ×ＴＳＬ毎に最大１個のＮｏ１ブロックを抽出することで、同じ探索エリアから２個以上のＮｏ１ブロックを抽出することなく、撮像画面の特定の領域に抽出箇所が集中しないようにする。
【００８３】
次に、ステップS32で、探索エリアＷＳ×ＴＳＬの中から１２×６ピクセルの絵柄箇所を切り出し、切り出し箇所の画像品質を評価してＮｏ１ブロックを決定する。１２×６ピクセルの絵柄箇所の画像品質は、対応する距離画像の１２×６ピクセルの領域の距離情報を下記の条件で選別してヒストグラムを作成し、このヒストグラムによって良否を判定する。
【００８４】
すなわち、１２×６ピクセルの距離画像値を所定の閾値に照らし合わせ、個々のデータの数値が閾値に定める許容範囲内の数値なら「ＯＫ情報」として度数をカウントし、ヒストグラムを作成する。また、許容範囲外なら「ＮＧ情報」として度数をカウントする。この「ＯＫ情報」と「ＮＧ情報」とを選別するための閾値は、１２×６ピクセルの距離画像データから目的に応じた遠方画像として認識できる数値を定める。
【００８５】
この場合、回転速度の計算を行う遠方画像からは、建物や山並み等の稜線を含んだＮｏ１ブロックが取得できることが望ましい。このため、 遠方用ステレオカメラ１２は、画面全体に対して走査線処理が速い領域を天側にし、走査線処理が遅い領域を地側に向けて設置しており、「ＯＫ情報」と「ＮＧ情報」との含有率を調整することにより、図１２に示すように、稜線を含んだ画像をＮｏ１ブロックＮ1Ｂn（ｎ＝0,1,2,…,9）として抽出できる。
【００８６】
具体的には、遠方画像の動きから回転速度成分を計算するため、距離画像データが多く存在する領域で、ズレ量が少なく（つまり、距離が遠い）、且つ、バラツキが小さい領域をＮｏ１ブロックの候補として閾値を定める。
【００８７】
すなわち、図１３に示すように、もし、１２×６ピクセルの距離画像領域の距離情報にバラツキがあると、これから定めようとするＮｏ１ブロックは、ある１箇所（１点）を見ているのではなく、同じ窓（小領域）の中に遠近が異なる複数箇所の対象物Ｍ，Ｅ１，Ｅ２を見ていることになり、ヒストグラム中のｅ１，ｅ２に対象物Ｅ１，Ｅ２がノイズとして出現することになる。
【００８８】
従って、１２×６ピクセルの距離画像値を上記閾値でフィルタにかけてノイズを除去し、さらに、ＯＫ情報の中から最も出現頻度が多い距離画像値ｍａｘｚと、その最大ヒストグラム値ｍａｘｎｕｍを求め、この最大ヒストグラム値ｍａｘｎｕｍ近辺の他の距離画像値の出現頻度を調べることにより、図１４に示すように、使用する領域の対象物Ｍの候補を定めることができる。
【００８９】
本形態では、ｍａｘｚ±２の範囲に絞って１２×６ピクセルの距離画像領域に含まれる距離画像値と、それぞれのヒストグラム値を調査するようにしており、ｍａｘｚ±２の範囲の出現頻度の総度数ｄｓｕｍを以下の(1)式で算出し、「ＯＫ情報」の度数が所定の数を満足し、且つ、ｍａｘｚ±２の範囲の総度数ｄｓｕｍが「ＯＫ情報」の度数の内の大部分を占める（例えば、８０％以上）とき、切り出した１２×６ピクセルの絵柄箇所は信頼性のある画像と判断し、Ｎｏ１ブロックとして決定する。
ｄｓｕｍ＝ｍ２＋ｍ１＋ｍａｘｎｕｍ＋ｐ１＋ｐ２ …(1)
但し、ｍ２：ｍａｘｚ−２における出現頻度
ｍ１：ｍａｘｚ−１における出現頻度
ｐ１：ｍａｘｚ＋１における出現頻度
ｐ２：ｍａｘｚ＋２における出現頻度
【００９０】
次に、Ｎｏ１ブロックの絵柄抽出が決定すると、航法計算に必要な対象物までの距離値を正確に知るため、１２×６ピクセルの距離画像領域の平均距離画像値ｆｚ1を以下の(2)式によって求める。
ｆｚ1＝ｚｓｕｍ／ｄｓｕｍ …(2)
但し、ｚｓｕｍ＝(ｍａｘｚ−２)・ｍ２＋(ｍａｘｚ−１)・ｍ１
＋ｍａｘｚ・ｍａｘｎｕｍ
＋(ｍａｘｚ＋１)・ｐ１＋(ｍａｘｚ＋２)・ｐ２
【００９１】
以上の遠方画像に対するＮｏ１ブロック取得処理を、各探索エリアＷＳ×ＴＳＬ毎に行い、画像全体で最大１０個のＮｏ１ブロックを取得すると、ステップS33以降の下方処理に移行する。尚、探索エリアＷＳ×ＴＳＬで適正なＮｏ１ブロックが見つからないときには、該当する探索エリアでの抽出を中止する。
【００９２】
下方処理では、ステップS33において、下方画像に対するイニシャル処理として、Ｎｏ１ブロックを探す走査範囲を設定する。この下方画像の走査範囲は、遠方画像の１０個の探索エリアＷＳ×ＴＳＬに対し、図１５に示すように、垂直走査方向をＴＡとした領域ＷＳ×ＴＡで、画面の中央ラインＴｈから進行方向側に所定量オフセットさせたラインＴｔを探索開始ラインとする領域であり、画面全体に分散した特徴的な絵柄を抽出できるよう、垂直走査方向の幅ＴＡを３分割して３０個の探索エリアＷＳ×ＴＳＧを設定する。
【００９３】
この場合、上記オフセットラインＴｔは、前回処理で求めた並進速度によって可変するようにしており、下方画像の動きを考慮し、前・後進速度に応じて撮像面を有効に利用するようにしている。
【００９４】
次に、ステップS34では、探索エリアＷＳ×ＴＳＧの中から１２×６ピクセルの絵柄箇所を切り出し、切り出し箇所の画像品質を評価してＮｏ１ブロックを決定する。この下方画像におけるＮｏ１ブロックの決定は、前述した遠方画像におけるＮｏ１ブロックの決定と同様であるが、「ＯＫ情報」と「ＮＧ情報」とを選別するための閾値が異なる。すなわち、下方画像の動きから並進速度成分を計算するため、距離画像データが多く存在する領域で、且つ、バラツキが小さい平面的な領域をＮｏ１ブロックの候補として閾値を定める。
【００９５】
そして、上記ステップS34において、水平走査方向に最大１０個のＮｏ１ブロックを取得すると、ステップS35へ進んで下方画像における全ての探索エリアの処理が済んだか否かを調べ、全ての処理が終了していない場合には、ステップS34へ戻って、一段下の探索エリアに対する処理を続行する。そして、このステップS34→ステップS35→ステップS34のループを３回繰り返して３０個の探索エリアについての処理を行う。
【００９６】
この場合においても、探索エリアＷＳ×ＴＳＧで適正なＮｏ１ブロックが見つからないときには、該当する探索エリアでの抽出を中止する。図１５では、下方画像について、Ｎｏ１ブロックをＮ１Ｂ0,1,2,…,22,23,24までの計２５個取得した例を示している。
【００９７】
以上により、遠方画像・下方画像のそれぞれについてＮｏ１ブロック群を取得すると、Ｎｏ１ブロックと同じ絵柄を持つＮｏ２ブロックを探す処理に移行する。航法計算の品質を確保するためには、Ｎｏ１ブロックの取得数分ｎ個を評価し、できるだけ多く（最大ｎ個）のＮｏ２ブロックを探し出す必要がある。ｎ個のＮｏ１・Ｎｏ２ブロックは、それぞれペアの関係であり、ペアにならなかったＮｏ１ブロックは排除し、新たなＮｏ１・Ｎｏ２ブロック群を求める。
【００９８】
以下、図８に示すＮｏ２ブロック群の取得処理について説明する。この処理では、ステップS40でＮｏ１ブロック取得数の評価を行い、所定の取得数に達していないときには不合格と判定してルーチンを抜け、所定の取得数に達して合格判定のときには、ステップS41へ進んで遠方・下方処理の判定を行う。
【００９９】
そして、遠方画像に対する処理のときには、上記ステップS41からステップS42へ進んで、遠方用ステレオカメラ１２の焦点距離、２台のカメラの間隔、無限遠方でのズレ量（視差）等の距離の算出に必要な遠方処理定数値を設定してステップS44へ進み、下方画像に対する処理のときには、上記ステップS41からステップS43へ進み、同様に、下方用ステレオカメラ１３の撮像画像から距離を算出するのに必要な下方処理定数値を設定してステップS44へ進む。
【０１００】
ステップS44では、Ｎｏ２ブロックの探索エリアを設定する。このＮｏ２ブロックの探索エリアは、撮像画像全体からＮｏ１ブロックと同じ絵柄を探すのではなく、所定時間経過後のＮｏ１ブロックの所在を予測して設定する。
【０１０１】
すなわち、Ｎｏ１ブロックが決定すると、Ｎｏ１ブロックの撮像面での代表座標が決まるため、まず、図１６に示すように、Ｎｏ１ブロックの代表座標を中心として、上下左右に所定のオフセット量（ｘｆ，ｙｆ）を与えた４点ａ，ｂ，ｃ，ｄを決定する。そして、この４点ａ，ｂ，ｃ，ｄを結んだ４角形の枠内すなわち（２・ｘｆ）×（２・ｙｆ）の領域を、画面に動きが無いときの撮像情報でＮｏ１ブロックと同じ絵柄が存在すると予測した探索エリアとし、この探索エリアを、遠方・下方画像のそれぞれの特徴を考慮して配置する。
【０１０２】
遠方画像の場合には、図１７に示すように、回転・並進が伴わない基準画面Ｇ０に対し、短時間の並進では、画面Ｇ１に示すように、遠方の雲ｍ1及び山ｍ2は変化がほとんど無く、近くの木立ｍ3が横方向へ移動するのみであるが、回転成分ヨーが生じたときの画面Ｇ２では、雲ｍ1、山ｍ2、木立ｍ3を含めた画面全体が横方向へ移動して、木立ｍ3の一部が視野から外れる。また、回転成分ロールが生じたときの画面Ｇ３では画面全体が傾き、回転成分ピッチが生じたときの画面Ｇ４では画面全体が下方に移動する。
【０１０３】
すなわち、遠方画像の場合には、測定点から十分遠く（例えば、３０ｍ以上）離れた箇所を測距するため、短時間での上下・左右の並進移動による画像の変化を受け難く、遠方画像で得たＮｏ１ブロックの動きは、回転成分による動きとして捉えられる。従って、遠方画像では、図１８に示すように、原点Ｏの回りの回転角αによって探索エリアの４点ａ，ｂ，ｃ，ｄの座標値を変換し、Ｎｏ１ブロックＮ1Ｂ0,Ｎ1Ｂ1,Ｎ1Ｂ2,…に対して４点ａn，ｂn，ｃn，ｄn（ｎ＝0,1,2,…）を結んだ４角形の探索エリアＳ0,Ｓ1,Ｓ2,…を設定する。そして、各探索エリアから以降のステップの処理を経てＮｏ２ブロックＮ2Ｂ0,Ｎ2Ｂ1,Ｎ2Ｂ2,…を抽出する。
【０１０４】
一方、下方画像の場合には、図１９に示すように、撮像面上の移動量Ｖｗは、並進成分Ｖａ、回転成分Ｖｒのどちらの動きで生じたものか判断し難い。このため、一定時間経過後のＮｏ１ブロックの所在を予測し、その移動先に、次フレーム以降のＮｏ２ブロックの検索エリアを設定する。すなわち、ヘリコプターの動きに対して処理速度が十分に速いときや、実撮像範囲が広いときには、画面毎の画像の動きは大きくないため、探索エリアは単位時間当たりの最大変化量を探索範囲と置けば良いが、ヘリコプターが大きく動く場合には、処理速度や撮像範囲が十分でないと仮定して動きを予測し、過去の速度成分を参考にして探索エリアを設定する。
【０１０５】
一定時間経過後のＮｏ１ブロックの所在を予測するには、前回の角速度計算で得られたロール・ピッチ・ヨー成分と前回の並進速度計算で得られた純並進量とから、所定時間後のＮｏ１ブロックの移動先を撮像面座標の動き（仮想的なオプティカルフロー）に変換する。そして、図２０に示すように、Ｎｏ１ブロックＮ1Ｂ0,Ｎ1Ｂ1,Ｎ1Ｂ2,Ｎ1Ｂ3,Ｎ1Ｂ4,…を決定したフレームＦｍとＮｏ２ブロックＮ2Ｂ0,Ｎ2Ｂ1,Ｎ2Ｂ2,Ｎ2Ｂ3,Ｎ2Ｂ4,…を探し出す対象フレームＦｍ＋１との間の仮想的なオプティカルフローＯＰＦＬに基づいて、フレームＦｍ＋１上にＮｏ２ブロックの探索エリアＳ0,Ｓ1,Ｓ2,Ｓ3,Ｓ4,…を設定する。
【０１０６】
以上のようにしてＮｏ２ブロックの探索エリアを設定すると、ステップS44からステップS45へ進み、Ｎｏ２ブロックの探索エリアからＮｏ１ブロックと同じ形状で元画像の輝度を最小単位で（１ピクセル毎に）抽出し、Ｎｏ１ブロックの対応する画素の輝度との差の絶対値をとって総和する（シティブロック距離）。そして、このシティブロック距離が最小値となる領域を、Ｎｏ２ブロックの候補地として探索エリア内から選別し、該当する撮像面座標を求めてステップS46へ進む。
【０１０７】
ステップS46では、Ｎｏ２ブロックの候補地について、Ｎｏ１ブロックの場合と同様に、フィルタ処理及びヒストグラム処理によってＮｏ２ブロックの元画像の信頼性を評価し、合格のとき、ステップS47でＮｏ２ブロックの取得処理を行い、不合格のときには該当データを破棄してステップS48へジャンプする。
【０１０８】
ステップS47におけるＮｏ２ブロックの取得処理では、必要に応じてサブピクセル処理を行ってサブピクセル値（１ピクセル以下の値）を求め、このサブピクセル値を含めたＮｏ２ブロックの撮像面座標値を計算する。そして、Ｎｏ２ブロックの１２×６ピクセル距離画像領域の平均距離画像値ｆｚ2を求め、Ｎｏ２ブロック取得数Ｎｕｍ２を記録してステップS48へ進む。
【０１０９】
ステップS48では、Ｎｏ１ブロック・Ｎｏ２ブロックの最終取得数を更新し、Ｎｏ１ブロック群及びＮｏ２ブロック群のデータをパッキング処理により圧縮して序列化する。そして、ステップS49でＮｏ２ブロックの取得処理回数がＮｏ１ブロックの取得数に達したか否かを調べ、Ｎｏ２ブロック取得処理回数＜Ｎｏ１ブロック取得数のときには、ステップS44へ戻るループとなり、Ｎｏ２ブロック取得処理の回数がＮｏ１ブロックの取得数に達したとき、処理を終了する。
【０１１０】
次に、遠方画像の動きから回転速度成分を演算する処理について説明する。図９の回転演算処理ルーチンでは、まず、ステップS50で、計算精度を維持するのに必要な所定のサンプル数だけＮｏ２ブロック群を取得できたか否かを調べ、所定の数を取得できないとき、ステップS55で前回の角速度成分の計算結果を今回処理の値としてメモリを更新し、ステップS56へ進む。
【０１１１】
一方、計算精度を維持するに必要な所定のサンプル数だけＮｏ２ブロック群を取得できたときには、上記ステップS50からステップS51へ進み、Ｎｏ１ブロック群の配列パターンとＮｏ２ブロック群の配列パターンとを比較してオプティカルフローの信頼性を評価する。
【０１１２】
すなわち、遠方画像の性質から、回転速度成分によるＮｏ１ブロック→Ｎｏ２ブロックの理想的な画像の動きでは、各ブロックの相対位置関係（配列パターン）は殆ど変わらない。従って、Ｎｏ１ブロック群の配列パターンとＮｏ２ブロック群の配列パターンとを比較し、各ブロックの配置が明らかに異なる場合には、該当するブロックをＮｏ１・Ｎｏ２ブロック群について共に排除する。
【０１１３】
この配列パターンの評価は、Ｎｏ１ブロック群における各ブロック間の距離及び各ブロックで構成する三角形の面積を、Ｎｏ２ブロック群における値と比較することで行う。例えば、一つのブロックＡについて着目すると、ブロックＡがＮｏ２ブロック群において正しい位置に選択されたか否かを評価するには、まず、Ｎｏ１ブロック群におけるブロックＡと他のブロックＢとの距離（Ａ１−Ｂ１）を、Ｎｏ２ブロック群におけるブロックＡとブロックＢとの距離（Ａ２−Ｂ２）と比較し、同様に、ブロックＡ−ブロックＣ、ブロックＡ−ブロックＤについても距離を比較する。
【０１１４】
そして、３つともＮｏ１ブロック群とＮｏ２ブロック群とにおける距離が等しければ距離のチェックはＯＫとし、そうでなければ、次のブロックＥ，Ｇ，…と順次比較するブロックを変えてゆき、先に３個ＯＫになれば距離のチェックはＯＫ、先に３個ＮＧになれば距離のチェックはＮＧとする。
【０１１５】
このように、ブロックＡの３点（Ｂ，Ｃ，Ｄ）に対する距離のチェックがＯＫになった場合、次に、ブロックＡ、Ｂ，Ｃの各代表座標で構成される三角形の面積ＡＢＣをＮｏ１ブロック群とＮｏ２ブロック群とについて求め、大きさを比較する。この比較を三角形ＡＣＤについても行い、どちらもＮｏ１ブロック群とＮｏ２ブロック群とで同じ面積であれば、最終的に、ブロックＡの位置はＮｏ２ブロック群において正しく選択されたと判定する。
【０１１６】
この際、距離のチェックは、ピタゴラスの定理による二乗値のまま比較し、また、三角形の面積のチェックは、ベクトルの外積を計算して三角形の面積の２倍の値（平行四辺形の面積）で比較することにより、計算効率を高める。
【０１１７】
以上の配列パターンの評価により適正でないブロックを排除した後、ステップS52へ進み、適正でないブロックを排除した後のＮｏ２ブロック数が所定の数か否かを調べる。そして、所定の数に達していないときには、上記ステップS52から前述のステップS55を経てステップS56へ進み、所定の数のＮｏ２ブロックを確保できたときには、上記ステップS52からステップS53へ進む。
【０１１８】
ステップS53では、Ｎｏ２ブロックの撮像面座標を、カメラ固定のＸ，Ｙ，Ｚ直交座標である実空間座標に直す。図２１に示すように、レンズの焦点距離ｆ、メインカメラ軸Ｚｍとサブカメラ軸Ｚｓとの基線長ＬS（Ｓｓ）、ステレオ画像のズレ量Ｘｒとすると、測距対象点Ｍまでの距離Ｚは、三角測量によって以下の(3)式で与えられるため、この関係に基づいて撮像面座標を実空間座標に変換する。
Ｚ＝ＬS（ＳS）・ｆ／Ｘｒ …(3)
【０１１９】
続くステップS54では、非線形型最小二乗法によってＮｏ１ブロック群とＮｏ２ブロック群との間の位置の差分に相当する角速度（ロール・ピッチ・ヨー）を求める。すなわち、前回の回転・並進の速度成分を初期値として撮像素子上の動きに換算してＮｏ１ブロックとＮｏ２ブロックとの位置の差を求め、さらに、線形近似を行って３軸（Ｘ，Ｙ，Ｚ軸）の回転成分毎に偏微分し、適当な偏角を与えてロール・ピッチ・ヨー毎に撮像素子上のＮｏ１ブロックとの位置の差の変化量（ｄｘ，ｄｙ）を求める。ロール・ピッチ・ヨーの各成分は、この変化量（ｄｘ，ｄｙ）の極限として与えられる。
【０１２０】
従って、上記変化量（ｄｘ，ｄｙ）を求める処理をＮｏ１・Ｎｏ２ブロック全てについて行い、最小二乗法で回転成分を求める。この最小二乗法では、３元１次方程式を解くことになるが、係数行列についての転置行列が等しく、対角要素が他の要素に比べて十分に大きい値であるため（正定値対称）、コレスキー法による数値計算を適用することができ、Ｎｏ１ブロックに対してのロール・ピッチ・ヨーの各々の値を求めることができる。
【０１２１】
実際には、上記手法による解は、非線形のため真値にはならない。従って、得られた値を初期値にして再び最小二乗法を繰り返す非線形型最小二乗法によって真値に近づけるわけであるが、真値を求める際には、上記変化量（ｄｘ，ｄｙ）の収束具合（理想的には０）を監視して決定すべき処置が必要である。すなわち、限定した範囲の角速度成分を適度な精度で得る場合には、繰り返し計算回数を所定の回数（例えば、５回）で打ち切り、処理に要する時間を短縮することができる。
【０１２２】
上記ステップS54の処理によってカメラ固定のＸＹＺ直交座標系における角速度成分を求めると、次に、ステップS56へ進み、ロール成分ｒｌ、ピッチ成分ｐｔ、及び、ヨー成分ｙｗを、回転行列Ｒｏｔの要素に展開してルーチンを抜ける。この回転行列Ｒｏｔは、ロール、ピッチ、ヨーの各軸毎の回転行列の積であり、例えば、以下の(4)〜(12)式で示すように、３×３の正方行列の要素として展開できる。
Ｒｏｔ[0][0]＝(ｃｙ・ｃｒ)−(ｓｙ・ａｎｓ) …(4)
Ｒｏｔ[1][0]＝−(ｃｐ・ｓｒ) …(5)
Ｒｏｔ[2][0]＝(ｓｙ・ｃｒ)＋(ｃｙ・ａｎｓ) …(6)
Ｒｏｔ[0][1]＝(ｃｙ・ｓｒ)＋(ｓｙ・ａｎｓ') …(7)
Ｒｏｔ[1][1]＝(ｃｐ・ｃｒ) …(8)
Ｒｏｔ[2][1]＝(ｓｙ・ｓｒ)−(ｃｙ・ａｎｓ') …(9)
Ｒｏｔ[0][2]＝−(ｓｙ・ｃｐ) …(10)
Ｒｏｔ[1][2]＝ｓｐ …(11)
Ｒｏｔ[2][2]＝(ｃｙ・ｃｐ) …(12)
但し、ｃｙ＝ｃｏｓ(ｙｗ) ｃｒ＝ｃｏｓ(ｒｌ)
ｃｐ＝ｃｏｓ(ｐｔ) ｓｙ＝ｓｉｎ(ｙｗ)
ｓｒ＝ｓｉｎ(ｒｌ) ｓｐ＝ｓｉｎ(ｐｔ)
ａｎｓ＝ｓｐ・ｓｒ ａｎｓ'＝ｓｐ・ｃｒ
【０１２３】
次に、回転速度成分と並進速度成分とを含む下方画像の動きから純並進速度成分を求める処理について説明する。
【０１２４】
図１０の並進演算処理ルーチンでは、ステップ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で原点（測距開始地点）から見た移動体の姿勢状態を求める。原点から見た移動体の姿勢状態は、測距開始地点で空間に固定したＸＹＺ直交座標系から見た回転行列Ｒｏｔｓによって示される。このため、まず、測距開始地点で回転行列を単位行列化して初期化することで撮像面に直交する平面を実空間を表す基準面とし、この初期化時点の行列にフレーム間の回転行列（カメラ固定のＸＹＺ直交座標系から見た回転行列）Ｒｏｔを右から掛け込んだ行列を原点から見た移動体の姿勢を表す回転行列Ｒｏｔｓとする。
【０１３２】
すなわち、原点から見た姿勢を表す回転行列Ｒｏｔｓは、カメラ固定の座標系から見たフレーム毎の回転行列Ｒｏｔを時系列的に積算した正方行列であり、以下の(13)式に示すように、前回までの原点から見た姿勢状態を表す回転行列Ｒｏｔｓに今回の回転演算処理で求めたフレーム間の回転行列Ｒｏｔを右から積算した正方行列で今回処理における原点から見た姿勢を表す回転行列Ｒｏｔｓとして更新することにより、原点から見た移動体の最新の姿勢を表すことができる。
【０１３３】
Ｒｏｔｓ＝Ｒｏｔｓ・Ｒｏｔ …(13)
【０１３４】
次に、ステップS81へ進み、フレーム間の並進速度成分を原点から見た並進速度成分に変換すると、ステップS82で、原点から見た速度成分をフレーム毎に累積し、イニシャル地点からの移動体の実空間座標を求める。
【０１３５】
原点から見た並進速度Ｓｏは、原点から見た姿勢を表す回転行列Ｒｏｔｓとフレーム間の３次元並進速度Ｓとの積を、以下の(17)式に示すように、前回までの原点から見た並進速度Ｓｏに累積することで求めることができ、累積したベクトル量に所定の距離換算値を掛け合わせることで、図２３に示すような、航法軌跡を得ることができ、原点からの移動体の移動量を知ることができる。
Ｓｏ＝Ｓｏ＋(Ｒｏｔｓ)・Ｓ …(17)
【０１３６】
この場合、地上の適当な地点に、図２２に示すようなランドマークを設置しておき、このランドマークをパターン認識等によって識別し、測距開始地点として原点をイニシャルすることで、上記航法軌跡から自己の相対位置を認識することができる。
【０１３７】
さらには、ＧＰＳ等の衛星測位システムからの測位情報や既知の地図情報によって地上の適当な地点を原点としてイニシャルすることで、航法軌跡から自己の絶対位置を認識することができる。
【０１３８】
以上により、撮像対象物までの距離を細かく認識して、極めて正確な測量を行うことができ、複雑な地形での低空飛行においても高精度な対地高度が得られる。すなわち、従来に比較してドリフトの少ない高精度の自律航法機能を担うことが可能であり、膨大な情報量を有する画像ならではの周囲環境分析を航法処理に反映することができる。
【０１３９】
【発明の効果】
以上説明したように本発明によれば、一定時間毎に遠方風景と下方風景とを撮像し、それぞれの撮像画像の中から特徴的な絵柄の領域を第１のブロックとして複数箇所抽出し、次のタイミングで撮像した画像の中から第１のブロックと同じ絵柄を有する領域を第２のブロックとして探索すると、遠方風景の撮像画像における第１のブロックから第２のブロックへの画像の動きを、遠方風景の撮像画像をステレオ画像処理して求めた距離画像に基づいて撮像面座標を実距離に換算した実空間座標での移動量として求め、該移動量とフレーム間の時間経過とから、移動体のフレーム間のカメラ固定の座標系における回転速度成分を求め、また、下方風景の撮像画像における第１のブロックから第２のブロックへの画像の動きを、下方風景の撮像画像をステレオ画像処理して求めた距離画像に基づいて撮像面座標を実距離に換算した実空間座標での移動量として求め、該移動量とフレーム間の時間経過とから、移動体のフレーム間のカメラ固定の座標系における速度成分を求め、この速度成分から上記回転速度成分を除去して移動体のフレーム間のカメラ固定の座標系における並進速度成分を求める。そして、このフレーム間のカメラ固定の座標系における並進速度成分を測距開始地点で空間に固定した座標系における並進速度成分に変換し、この変換した並進速度成分を累積して３次元空間における航法軌跡を求め、移動体の自己位置を認識するため、ドリフトの少ない高精度の自律航法機能を担うことができ、膨大な情報量を有する画像ならではの周囲環境分析を航法処理に反映することができる等優れた効果が得られる。
【図面の簡単な説明】
【図１】自己位置認識装置の機能構成図
【図２】３次元空間計測用カメラアセンブリの構成図
【図３】自己位置認識装置の回路ブロック図
【図４】並列処理のタイムチャート
【図５】ＳＹＳ１初期化処理ルーチンのフローチャート
【図６】ＳＹＳ２初期化処理ルーチンのフローチャート
【図７】Ｎｏ１ブロック群取得処理ルーチンのフローチャート
【図８】Ｎｏ２ブロック群取得処理ルーチンのフローチャート
【図９】回転演算処理ルーチンのフローチャート
【図１０】並進演算処理ルーチンのフローチャート
【図１１】航法演算処理ルーチンのフローチャート
【図１２】遠方のＮｏ１ブロックの取得パターンを示す説明図
【図１３】画像評価におけるヒストグラム分布の好ましくない例を示す説明図
【図１４】画像評価におけるヒストグラム分布の好ましい例を示す説明図
【図１５】下方のＮｏ１ブロックの取得パターンを示す説明図
【図１６】探索エリアの設定を示す説明図
【図１７】遠方画像の特徴を示す説明図
【図１８】遠方画像に探索エリアを設定した例を示す説明図
【図１９】下方の撮像面上の動きを示す説明図
【図２０】下方画像に探索エリアを設定した例を示す説明図
【図２１】三角測量による撮像面座標から実距離への換算を示す説明図
【図２２】測距開始地点のランドマークを示す説明図
【図２３】自律航法による航法軌跡の説明図
【符号の説明】
１０…カメラアセンブリ
２０…ステレオ画像処理装置
３０…自己位置認識装置

Claims (21)

1. 移動体に搭載されるステレオカメラにより、一定時間毎に遠方風景と下方風景とを撮像する手段と、
   遠方風景の撮像画像と下方風景の撮像画像とに対し、それぞれの撮像画像の中から特徴的な絵柄の領域を第１のブロックとして複数箇所抽出し、次のタイミングで撮像した画像の中から第１のブロックと同じ絵柄を有する領域を第２のブロックとして探索する手段と、
   遠方風景の撮像画像における第１のブロックから第２のブロックへの画像の動きを、遠方風景の撮像画像をステレオ画像処理して求めた距離画像に基づいて撮像面座標を実距離に換算した実空間座標での移動量として求め、該移動量とフレーム間の時間経過とから、移動体のフレーム間のカメラ固定の座標系における回転速度成分を求める手段と、
   下方風景の撮像画像における第１のブロックから第２のブロックへの画像の動きを、下方風景の撮像画像をステレオ画像処理して求めた距離画像に基づいて撮像面座標を実距離に換算した実空間座標での移動量として求め、該移動量とフレーム間の時間経過とから、移動体のフレーム間のカメラ固定の座標系における速度成分を求め、この速度成分から上記回転速度成分を除去して移動体のフレーム間のカメラ固定の座標系における並進速度成分を求める手段と、
   上記フレーム間のカメラ固定の座標系における並進速度成分を測距開始地点で空間に固定した座標系における並進速度成分に変換し、この変換した並進速度成分を累積して３次元空間における航法軌跡を求めることにより、移動体の自己位置を認識する手段とを備えたことを特徴とする移動体の３次元自己位置認識装置。
2. 上記フレーム間のカメラ固定の座標系における並進速度成分を、測距開始地点で空間に固定した座標系の原点から見た移動体の最新の姿勢を表す回転成分で補正し、測距開始地点で空間に固定した座標系における並進速度成分に変換することを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
3. 測距開始地点で空間に固定した座標系における移動体の姿勢を表す行列を単位行列化して初期化した後、今回処理時のフレーム間のカメラ固定の座標系における回転速度成分を表す行列を測距開始地点で空間に固定した座標系の原点から見た前回までの移動体の姿勢を表す行列に積算し、上記移動体の最新の姿勢を表す回転成分とすることを特徴とする請求項２記載の移動体の３次元自己位置認識装置。
4. 第１のブロックを抽出する際、撮像画面を所定範囲で分割した複数の探索エリアを設定し、各探索エリア毎に第１のブロックを最大１個抽出することを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
5. 第１のブロックをｍ個抽出する際、撮像画面をｎ個の領域に分割し、各領域で抽出する第１のブロックを最大ｍ／ｎ個とすることを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
6. 第１のブロックを遠方風景の撮像画像から抽出する際、上記距離画像の対応する領域の距離画像値が小さく、測距点から十分に遠方にある小領域を、第１のブロックの候補とすることを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
7. 第１のブロックを下方風景の撮像画像から抽出する際、上記距離画像の対応する領域の距離画像値のバラツキが少ない平面的な小領域を、第１のブロックの候補とすることを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
8. 第１のブロックを下方風景の撮像画像から抽出する際、画面の中央ラインから進行方向側に所定量オフセットさせたラインを探索開始ラインとして設定し、この探索開始ラインを上記並進速度成分によって可変することを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
9. 第１のブロックを抽出する際、対象とする領域の画像品質を、上記距離画像の対応する領域の距離画像値を用いて作成したヒストグラムによって評価することを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
10. 上記ヒストグラムを、抽出ピクセルが所定の閾値以上の度数をカウントして作成することを特徴とする請求項９記載の移動体の３次元自己位置認識装置。
11. 最大ヒストグラムを記録する距離画像値に対して所定範囲にあるヒストグラム値の総度数が所定の比率以上のとき、対象とする領域の画像品質が良好であると判定することを特徴とする請求項９記載の移動体の３次元自己位置認識装置。
12. 第２のブロックを探索する際、第１のブロックを囲む所定の探索エリアを設定し、この探索エリアから第２のブロックを探索することを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
13. 第２のブロックを探索する際、前回の回転速度成分あるいは並進速度成分から次の画面での第１のブロックの移動先の撮像面座標を予測し、予測した撮像面座標を囲む所定の探索エリアから第２のブロックを探索することを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
14. 第２のブロックを探索する際、第１のブロックを囲む所定の探索エリアを設定し、この探索エリアと第１のブロックとの間でシティブロック距離が最小となる領域を第２のブロックの候補とすることを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
15. 第２のブロックを探索する際、前回の回転速度成分あるいは並進速度成分から次の画面での第１のブロックの移動先の撮像面座標を予測し、予測した撮像面座標を囲む所定の探索エリアと第１のブロックとの間でシティブロック距離が最小となる領域を第２のブロックの候補とすることを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
16. 第１のブロックと第２のブロックとの対を所定数取得できないとき、前回処理で求めた回転速度成分及び並進速度成分を今回処理の回転速度成分及び並進速度成分として更新することを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
17. 複数の第１のブロックの配列パターンと複数の第２のブロックの配列パターンとの相違を調べて配置が異なるブロックを排除した後、第１のブロックと第２のブロックとの対を所定数確保できたとき、上記フレーム毎の移動体の回転速度成分を求める処理へ移行し、第１のブロックと第２のブロックとの対を所定数確保できないときには、前回処理で求めた回転速度成分を今回処理の回転速度成分として更新することを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
18. 上記配列パターンの相違を、各ブロック間の距離と各ブロックによって構成される三角形の面積とによって調べることを特徴とする請求項１７記載の移動体の３次元自己位置認識装置。
19. 上記測距開始地点の位置を、測位装置からの情報あるいは既知の地図情報によって初期化し、上記航法軌跡から移動体の絶対位置を認識することを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
20. 上記ステレオカメラを、遠方風景撮像用のステレオカメラと下方風景撮像用のステレオカメラとから構成し、互いのステレオカメラの撮像面の方向が直交するよう配置することを特徴とする請求項１記載の移動体の３次元自己位置認識装置。
21. 上記遠方風景撮像用のステレオカメラと上記下方風景撮像用のステレオカメラとを、互いの基準カメラ同士が近接した状態に配置することを特徴とする請求項２０記載の移動体の３次元自己位置認識装置。

Images