JPH1151650A

JPH1151650A - 移動体の３次元自己位置認識装置

Info

Publication number: JPH1151650A
Application number: JP9209319A
Authority: JP
Inventors: Keiji Saneyoshi; 敬二実吉
Original assignee: Fuji Heavy Industries Ltd
Current assignee: Subaru Corp
Priority date: 1997-08-04
Filing date: 1997-08-04
Publication date: 1999-02-26
Anticipated expiration: 2017-08-04
Also published as: EP0896267B1; DE69836522T2; US6025790A; JP3833786B2; EP0896267A2; DE69836522D1; EP0896267A3

Abstract

(57)【要約】【課題】３次元空間における周囲環境を分析して航法
情報に反映させ、高精度で緻密な航法情報を得る。【解決手段】カメラアセンブリ１０によって遠方風景
及び下方風景を一定時間毎に撮像し、自己位置認識装置
３０で、遠方風景の撮像画像、下方風景の撮像画像のそ
れぞれについて、撮像時間の異なる画像間の動きから移
動体の運動を検出する。すなわち、遠方画像の動き、下
方画像の動きを、それぞれの距離画像に基づいて実空間
での移動量に換算し、下方画像の動きによる速度成分か
ら遠方画像の動きによる回転速度成分を除去して純並進
速度成分を求めた後、測距開始地点から見た並進速度成
分に変換して累積し、３次元空間における航法軌跡を求
めて移動体の自己位置を認識する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、自律航法によって
移動する移動体の３次元自己位置認識装置に関する。

【０００２】

【従来の技術】従来より、無人ロボット、自律走行作業
車、無人ヘリコプター等の自律的に移動する移動体に関
し、移動制御、経路検出、航路検出、ロケーション検出
等の各種技術が開発されており、これらの中でも、自己
位置認識は重要な技術の一つである。

【０００３】この自己位置認識の技術としては、例え
ば、自律走行作業車等の地上を自律的に走行する移動体
では、振動ジャイロや光ジャイロによって２次元の角速
度を検出するとともに、対地速度を測定するセンサによ
って並進速度を検出し、基準位置からの移動量を演算し
て自己位置を測定する技術があり、また、無人ヘリコプ
ター等の飛行移動体では、慣性航法装置により、重力加
速度を検出して飛行体の加速度を検出し、この加速度を
積分することで移動量を知る技術がある。

【０００４】さらに、近年では、ＧＰＳ衛星等のよう
に、少なくとも３つの人工衛星の電波を受信し、２台の
ＧＰＳ受信機で受ける電波の位相差あるいは単独で符号
通信内容（衛星の位置と電波の発射更新時間）を解析
し、自己位置を割り出す技術も採用されており、本出願
人は、先に、特願平８−２４９０６１号公報において、
衛星利用の測位データにより基準位置からの走行履歴に
基づく測位データを補正し、制御安定性を向上した自律
走行車を提案している。

【０００５】

【発明が解決しようとする課題】しかしながら、無人ヘ
リコプター等のような飛行体では、３次元空間における
自己の回転や並進の速度を知る必要があるばかりでな
く、低空飛行の際には正確な対地高度や木目細かな航法
情報を得る必要がある。

【０００６】このような場合、従来の装置では、測位精
度低下の原因となるドリフトの影響によって精度が不足
するばかりでなく、地形が複雑であると、正確な対地高
度を得ることは困難である。すなわち、従来の技術で
は、３次元空間における周囲環境を細かく分析し、航法
情報に反映させるという点で不十分である。

【０００７】本発明は上記事情に鑑みてなされたもの
で、３次元空間における周囲環境を分析して航法情報に
反映させ、高精度で緻密な航法情報を得ることのできる
移動体の３次元自己位置認識装置を提供することを目的
としている。

【０００８】

【課題を解決するための手段】請求項１記載の発明は、
移動体に搭載される撮像系により、一定時間毎に遠方風
景と下方風景とを撮像する手段と、遠方風景の撮像画像
と下方風景の撮像画像とに対し、それぞれの撮像画像の
中から特徴的な絵柄の領域を第１のブロックとして複数
箇所抽出し、次のタイミングで撮像した画像の中から第
１のブロックと同じ絵柄を有する領域を第２のブロック
として探索する手段と、遠方風景の撮像画像における第
１のブロックから第２のブロックへの画像の動きとフレ
ーム間の時間経過とから、移動体のフレーム間の回転速
度成分を求める手段と、下方風景の撮像画像における第
１のブロックから第２のブロックへの画像の動きとフレ
ーム間の時間経過とから、移動体のフレーム間の速度成
分を求め、この速度成分から上記回転速度成分を除去し
て移動体のフレーム間の並進速度成分を求める手段と、
上記フレーム間の並進速度成分を測距開始地点から見た
並進速度成分に変換し、この変換した並進速度成分を累
積して３次元空間における航法軌跡を求めることによ
り、移動体の自己位置を認識する手段とを備えたことを
特徴とする。

【０００９】請求項２記載の発明は、請求項１記載の発
明において、上記撮像系をステレオ撮像系とし、上記画
像の動きを、遠方風景及び下方風景のステレオ撮像画像
から求めた距離画像に基づいて撮像面座標を実距離に換
算した実空間座標での移動量として求めることを特徴と
する。

【００１０】請求項３記載の発明は、請求項１記載の発
明において、上記フレーム間の並進速度成分を、測距開
始地点から見た移動体の最新の姿勢を表す回転速度成分
で補正して測距開始地点から見た並進速度成分に変換す
ることを特徴とする。

【００１１】請求項４記載の発明は、請求項３記載の発
明において、測距開始地点で移動体の回転速度成分を表
す行列を単位行列化して初期化した後、今回処理時のフ
レーム間の回転速度成分を表す行列を前回までの測距開
始地点から見た移動体の姿勢を表す行列に積算し、測距
開始地点から見た移動体の最新の姿勢を表す行列とする
ことを特徴とする。

【００１２】請求項５記載の発明は、請求項１記載の発
明において、第１のブロックを抽出する際、撮像画面を
所定範囲で分割した複数の探索エリアを設定し、各探索
エリア毎に第１のブロックを最大１個抽出することを特
徴とする。

【００１３】請求項６記載の発明は、請求項１記載の発
明において、第１のブロックをｍ個抽出する際、撮像画
面をｎ個の領域に分割し、各領域で抽出する第１のブロ
ックを最大ｍ／ｎ個とすることを特徴とする。

【００１４】請求項７記載の発明は、請求項２記載の発
明において、第１のブロックを遠方風景の撮像画像から
抽出する際、上記距離画像の対応する領域の距離画像値
が小さく、測距点から十分に遠方にある小領域を、第１
のブロックの候補とすることを特徴とする。

【００１５】請求項８記載の発明は、請求項２記載の発
明において、第１のブロックを下方風景の撮像画像から
抽出する際、上記距離画像の対応する領域の距離画像値
のバラツキが少ない平面的な小領域を、第１のブロック
の候補とすることを特徴とする。

【００１６】請求項９記載の発明は、請求項１記載の発
明において、第１のブロックを下方風景の撮像画像から
抽出する際、画面の中央ラインから進行方向側に所定量
オフセットさせたラインを探索開始ラインとして設定
し、この探索開始ラインを上記並進速度成分によって可
変することを特徴とする。

【００１７】請求項１０記載の発明は、請求項２記載の
発明において、第１のブロックを抽出する際、対象とす
る領域の画像品質を、上記距離画像の対応する領域の距
離画像値を用いて作成したヒストグラムによって評価す
ることを特徴とする。

【００１８】請求項１１記載の発明は、請求項１０記載
の発明において、上記ヒストグラムを、抽出ピクセルが
所定の閾値以上の度数をカウントして作成することを特
徴とする。

【００１９】請求項１２記載の発明は、請求項１０記載
の発明において、最大ヒストグラムを記録する距離画像
値に対して所定範囲にあるヒストグラム値の総度数が所
定の比率以上のとき、対象とする領域の画像品質が良好
であると判定することを特徴とする。

【００２０】請求項１３記載の発明は、請求項１記載の
発明において、第２のブロックを探索する際、第１のブ
ロックを囲む所定の探索エリアを設定し、この探索エリ
アから第２のブロックを探索することを特徴とする。

【００２１】請求項１４記載の発明は、請求項１記載の
発明において、第２のブロックを探索する際、前回の回
転速度成分あるいは並進速度成分から次の画面での第１
のブロックの移動先の撮像面座標を予測し、予測した撮
像面座標を囲む所定の探索エリアから第２のブロックを
探索することを特徴とする。

【００２２】請求項１５記載の発明は、請求項２記載の
発明において、第２のブロックを探索する際、第１のブ
ロックを囲む所定の探索エリアを設定し、この探索エリ
アと第１のブロックとの間でシティブロック距離が最小
となる領域を第２のブロックの候補とすることを特徴と
する。

【００２３】請求項１６記載の発明は、請求項２記載の
発明において、第２のブロックを探索する際、前回の回
転速度成分あるいは並進速度成分から次の画面での第１
のブロックの移動先の撮像面座標を予測し、予測した撮
像面座標を囲む所定の探索エリアと第１のブロックとの
間でシティブロック距離が最小となる領域を第２のブロ
ックの候補とすることを特徴とする。

【００２４】請求項１７記載の発明は、請求項１記載の
発明において、第１のブロックと第２のブロックとの対
を所定数取得できないとき、前回処理で求めた回転速度
成分及び並進速度成分を今回処理の回転速度成分及び並
進速度成分として更新することを特徴とする。

【００２５】請求項１８記載の発明は、請求項１記載の
発明において、複数の第１のブロックの配列パターンと
複数の第２のブロックの配列パターンとの相違を調べて
配置が異なるブロックを排除した後、第１のブロックと
第２のブロックとの対を所定数確保できたとき、上記フ
レーム毎の移動体の回転速度成分を求める処理へ移行
し、第１のブロックと第２のブロックとの対を所定数確
保できないときには、前回処理で求めた回転速度成分を
今回処理の回転速度成分として更新することを特徴とす
る。

【００２６】請求項１９記載の発明は、請求項１８記載
の発明において、上記配列パターンの相違を、各ブロッ
ク間の距離と各ブロックによって構成される三角形の面
積とによって調べることを特徴とする。

【００２７】請求項２０記載の発明は、請求項１記載の
発明において、上記測距開始地点の位置を、測位装置か
らの情報あるいは既知の地図情報によって初期化し、上
記航法軌跡から移動体の絶対位置を認識することを特徴
とする。

【００２８】請求項２１記載の発明は、請求項２記載の
発明において、上記ステレオ撮像系を、遠方風景撮像用
のステレオカメラと下方風景撮像用のステレオカメラと
から構成し、互いのステレオカメラの撮像面の方向が直
交するよう配置することを特徴とする。

【００２９】請求項２２記載の発明は、請求項２１記載
の発明において、上記遠方風景撮像用のステレオカメラ
と上記下方風景撮像用のステレオカメラとを、互いの基
準カメラ同士が近接した状態に配置することを特徴とす
る。

【００３０】すなわち、本発明による移動体の自己位置
認識装置では、一定時間毎に遠方風景と下方風景とを撮
像し、それぞれの撮像画像の中から特徴的な絵柄の領域
を第１のブロックとして複数箇所抽出し、次のタイミン
グで撮像した画像の中から第１のブロックと同じ絵柄を
有する領域を第２のブロックとして探索すると、遠方風
景の撮像画像における第１のブロックから第２のブロッ
クへの画像の動きとフレーム間の時間経過とから、移動
体のフレーム間の回転速度成分を求め、また、下方風景
の撮像画像における第１のブロックから第２のブロック
への画像の動きとフレーム間の時間経過とから、移動体
のフレーム間の速度成分を求め、この速度成分から上記
回転速度成分を除去して移動体のフレーム間の並進速度
成分を求める。そして、このフレーム間の並進速度成分
を測距開始地点から見た並進速度成分に変換し、この変
換した並進速度成分を累積して３次元空間における航法
軌跡を求め、移動体の自己位置を認識する。

【００３１】この場合、測距開始地点の位置を測位装置
からの情報あるいは既知の地図情報によって初期化する
ことで移動体の絶対位置を認識することができ、画像の
動きは、遠方風景及び下方風景のステレオ撮像画像から
求めた距離画像に基づいて撮像面座標を実距離に換算し
た実空間座標での移動量として求めることができる。ま
た、フレーム間の並進速度成分は、測距開始地点から見
た移動体の最新の姿勢を表す回転速度成分で補正して測
距開始地点から見た並進速度成分に変換することができ
る。

【００３２】測距開始地点から見た移動体の最新の姿勢
を表す回転速度成分は、測距開始地点で移動体の回転速
度成分を表す行列を単位行列化して初期化した後、今回
処理時のフレーム間の回転速度成分を表す行列を前回ま
での測距開始地点から見た移動体の姿勢を表す行列に積
算することで得られる。

【００３３】また、第１のブロックを抽出する際には、
撮像画面を所定範囲で分割した複数の探索エリアを設定
し、各探索エリア毎に第１のブロックを最大１個抽出す
る、あるいは、第１のブロックをｍ個抽出する際、撮像
画面をｎ個の領域に分割し、各領域で抽出する第１のブ
ロックを最大ｍ／ｎ個とすることが望ましく、撮像画面
の特定の領域に抽出箇所が集中せずに画面全体に分散し
た特徴的な絵柄を抽出することができる。

【００３４】また、第１のブロックを下方風景の撮像画
像から抽出する際には、画面の中央ラインから進行方向
側に所定量オフセットさせたラインを探索開始ラインと
して設定し、この探索開始ラインを上記並進速度成分に
よって可変することが望ましく、下方画像の動きを考慮
して前・後進速度に応じて撮像面を有効に利用すること
ができる。

【００３５】さらに、第２のブロックを探索する際に
は、第１のブロックを囲む所定の探索エリアを設定し、
この探索エリアから第２のブロックを探索することで、
限定した領域から確実な画像マッチングを行うことがで
き、特に、前回の回転速度成分あるいは並進速度成分か
ら次の画面での第１のブロックの移動先の撮像面座標を
予測し、予測した撮像面座標を囲む所定の探索エリアか
ら第２のブロックを探索することが望ましい。

【００３６】また、第１のブロックと第２のブロックと
の対を所定数取得できないときには、前回処理で求めた
回転速度成分及び並進速度成分を今回処理の回転速度成
分及び並進速度成分として更新する、あるいは、複数の
第１のブロックの配列パターンと複数の第２のブロック
の配列パターンとの相違を調べて配置が異なるブロック
を排除した後、第１のブロックと第２のブロックとの対
を所定数確保できたとき、フレーム毎の移動体の回転速
度成分を求める処理へ移行し、第１のブロックと第２の
ブロックとの対を所定数確保できないときには、前回処
理で求めた回転速度成分を今回処理の回転速度成分とし
て更新することが望ましく、誤った情報による誤差を排
除することができる。この配列パターンの相違は、各ブ
ロック間の距離と各ブロックによって構成される三角形
の面積とによって調べることができる。

【００３７】一方、距離画像を用いる場合には、第１の
ブロックを遠方風景の撮像画像から抽出する際、距離画
像の対応する領域の距離画像値が小さく、測距点から十
分に遠方にある小領域を第１のブロックの候補とし、第
１のブロックを下方風景の撮像画像から抽出する際、上
記距離画像の対応する領域の距離画像値のバラツキが少
ない平面的な小領域を第１のブロックの候補とすること
が望ましく、航法計算に適した特徴的な絵柄を抽出する
ことができる。

【００３８】また、距離画像を用いて第１のブロックを
抽出する際には、対象とする領域の画像品質を、距離画
像の対応する領域の距離画像値を用いて作成したヒスト
グラムによって評価することが望ましく、ヒストグラム
を、抽出ピクセルが所定の閾値以上の度数をカウントし
て作成し、最大ヒストグラムを記録する距離画像値に対
して所定範囲にあるヒストグラム値の総度数が所定の比
率以上のとき、対象とする領域の画像品質が良好である
と判定することが望ましい。

【００３９】また、第２のブロックを探索する際には、
第１のブロックを囲む所定の探索エリアを設定し、この
探索エリアと第１のブロックとの間でシティブロック距
離が最小となる領域を第２のブロックの候補とすること
が望ましく、特に、前回の回転速度成分あるいは並進速
度成分から次の画面での第１のブロックの移動先の撮像
面座標を予測し、予測した撮像面座標を囲む所定の探索
エリアと第１のブロックとの間でシティブロック距離が
最小となる領域を第２のブロックの候補とすることが望
ましい。

【００４０】さらに、ステレオ撮像系を、遠方風景撮像
用のステレオカメラと下方風景撮像用のステレオカメラ
とから構成し、互いのステレオカメラの撮像面の方向が
直交するよう配置することが望ましく、遠方風景撮像用
のステレオカメラと下方風景撮像用のステレオカメラと
を、互いの基準カメラ同士が近接した状態に配置するこ
とで、移動体の回転運動の検出中心と並進運動の検出中
心とを略一致させることができる。

【００４１】

【発明の実施の形態】以下、図面を参照して本発明の実
施の形態を説明する。図面は本発明の実施の一形態に係
わり、図１は自己位置認識装置の機能構成図、図２は３
次元空間計測用カメラアセンブリの構成図、図３は自己
位置認識装置の回路ブロック図、図４は並列処理のタイ
ムチャート、図５はＳＹＳ１初期化処理ルーチンのフロ
ーチャート、図６はＳＹＳ２初期化処理ルーチンのフロ
ーチャート、図７はＮｏ１ブロック群取得処理ルーチン
のフローチャート、図８はＮｏ２ブロック群取得処理ル
ーチンのフローチャート、図９は回転演算処理ルーチン
のフローチャート、図１０は並進演算処理ルーチンのフ
ローチャート、図１１は航法演算処理ルーチンのフロー
チャート、図１２は遠方のＮｏ１ブロックの取得パター
ンを示す説明図、図１３は画像評価におけるヒストグラ
ム分布の好ましくない例を示す説明図、図１４は画像評
価におけるヒストグラム分布の好ましい例を示す説明
図、図１５は下方のＮｏ１ブロックの取得パターンを示
す説明図、図１６は探索エリアの設定を示す説明図、図
１７は遠方画像の特徴を示す説明図、図１８は遠方画像
に探索エリアを設定した例を示す説明図、図１９は下方
の撮像面上の動きを示す説明図、図２０は下方画像に探
索エリアを設定した例を示す説明図、図２１は三角測量
による撮像面座標から実距離への換算を示す説明図、図
２２は測距開始地点のランドマークを示す説明図、図２
３は自律航法による航法軌跡の説明図である。

【００４２】図１は、３次元空間計測用の複数のカメラ
からなるカメラアセンブリ１０と、このカメラアセンブ
リ１０で撮像した画像から距離情報を取得するステレオ
画像処理装置２０と、画像情報から３次元空間における
移動体の自己位置を認識し、航法データを外部のターゲ
ットコントロールシステム１００に出力する自己位置認
識装置３０等から構成される移動体の３次元自己位置認
識装置であり、本形態においては、例えば、農薬散布等
に使用される無人ヘリコプターに搭載される自律航法装
置への適用例を示し、ハンドシェイクまたはタイムスケ
ジュールによる並列処理によってデータ処理が行われ
る。

【００４３】上記カメラアセンブリ１０は、移動体に搭
載されて周囲環境を一定時間毎に撮像し、周囲環境の変
化を１画面単位で捕らえるとともに、水平ズレに応じた
距離情報を取得するための測距センサ的役割を果たすも
のであり、図２に示すように、架台１１に、移動体の回
転速度成分を演算するために必要な遠方風景を撮像する
ための１組の遠方用ステレオカメラ１２と、移動体の並
進速度成分を演算するのに必要な下方風景（地表面）を
撮像するための１組の下方用ステレオカメラ１３とが設
置されている。

【００４４】上記遠方用ステレオカメラ１２は、メイン
カメラ（基準カメラ）１２ａと、このメインカメラ１２
ａと同じ仕様のサブカメラ１２ｂとが基線長ＬSの関係
で、且つ、互いの撮像面垂直軸が平行となるよう配置さ
れており、また、上記下方用ステレオカメラ１３は、メ
インカメラ（基準カメラ）１３ａと、このメインカメラ
１３ａと同じ仕様のサブカメラ１３ｂとが基線長ＳSの
関係で、且つ、互いの撮像面垂直軸が平行となるように
配置されている。

【００４５】本発明による自己位置認識装置では、遠方
風景の撮像画像及び下方風景の撮像画像のそれぞれにつ
いて、撮像時間の異なる画像間の動きから移動体の運動
を検出するようにしており、遠方画像の動き、下方画像
の動きを、それぞれの距離画像に基づいて実空間での移
動量に換算し、図２３に示すように、下方画像の動きに
よる速度成分から遠方画像の動きによる回転速度成分を
除去して純並進速度成分を求めた後、測距開始地点（ス
タート地点）から見た並進速度成分に変換して累積する
ことにより、３次元空間における航法軌跡を求めて移動
体の自己位置を認識する。

【００４６】この場合、遠方用ステレオカメラ１２によ
って移動体の回転速度を検出し、下方用ステレオカメラ
１３によって移動体の回転・並進速度を検出するために
は、遠方用ステレオカメラ１２の撮像面垂直軸と下方用
ステレオカメラ１３の撮像面垂直軸とが互いに直交し、
且つ、遠方のメインカメラ軸と下方のメインカメラ軸と
が同一平面で互いの基準点が一致していることが理想的
である。

【００４７】しかしながら、２つのメインカメラ１２
ａ，１３ａの基準点を同じにすることは現実的には困難
であるため、遠方用ステレオカメラ１２の撮像面垂直軸
と上記下方用ステレオカメラ１３の撮像面垂直軸とが互
いに直交するよう配置するとともに、遠方用のメインカ
メラ１２ａと下方用のメインカメラ１３ａとを寄せた配
置とし、一方のカメラの３軸が他方のカメラの３軸のい
ずれかと平行関係になるよう配置することで、２組のス
テレオカメラ１２，１３から得られる遠方画像及び下方
画像の動きに対し、１つの実空間座標系での取り扱いを
可能とする。

【００４８】この場合、下方用ステレオカメラ１３に３
軸中心を置くが、遠方用ステレオカメラ１２が下方用ス
テレオカメラ１３の周囲を回転し、両者のオフセット量
が生じても、後述するように、遠方画像の性質から、遠
方画像の動きには影響せず、下方のカメラ軸と遠方のカ
メラ軸とが直交関係にあることから、並進と回転を含む
速度成分から回転速度成分を除く処理や原点（測距開始
地点で空間に固定したＸＹＺ座標系原点）から見た航法
計算処理を簡素化し、画像の動きを的確に把握すること
を可能としている。

【００４９】尚、上記カメラアセンブリ１０に使用する
カメラは、ＥＩＡ準拠の白黒カメラの他、カラーＣＣＤ
カメラ（３−ＣＣＤ方式をも含む）、赤外線カメラ、暗
視カメラ等のエリア・スキャンを行う方式のカメラや、
撮像素子からの情報をデジタルで出力する方式のカメラ
等が使用可能である。

【００５０】上記ステレオ画像処理装置２０は、主とし
て、メインカメラ１２ａ（１３ａ）の撮像対象領域と同
じパターン（模様）を持つ領域箇所をサブカメラ１２ｂ
（１３ｂ）の撮像座標から探して撮像素子から対象物ま
での距離に応じて生じる画素のズレ（＝視差）を求め、
このズレ量から得られる対象物までの遠近情報を数値化
した３次元画像情報を取得する（ステレオ画像処理）も
のであり、遠方元画像メモリ２１、下方元画像メモリ２
２、遠方距離画像メモリ２３、下方距離画像メモリ２４
等の処理結果を情報内容別に記録するメモリを備えてい
る。

【００５１】上記遠方元画像メモリ２１には、遠方用の
メインカメラ１２ａで撮像した画像をＡＤ変換及びＬＯ
Ｇ変換して数値化した画像データが時系列的に記録さ
れ、上記下方元画像メモリ２２には、下方用のメインカ
メラ１３ａで撮像した画像をＡＤ変換及びＬＯＧ変換し
て数値化した画像データが時系列的に記録される。

【００５２】また、上記遠方用距離画像メモリ２３に
は、遠方用ステレオカメラ１２から得られる２つの画像
をステレオ画像処理し、メインカメラ１２ａで撮像した
撮像対象箇所または対象物までの距離値を数値化したデ
ータが時系列的に記録され、同様に、上記下方用距離画
像メモリ２４には、下方用ステレオカメラ１３から得ら
れる２つの画像をステレオ画像処理し、メインカメラ１
３ａで撮像した撮像対象箇所または対象物までの距離値
を数値化したデータが時系列的に記録される。

【００５３】尚、上記ステレオ画像処理については、本
出願人による特開平５−１１４０９９号公報に詳述され
ている。

【００５４】上記自己位置認識装置３０では、所定時間
毎に撮像した遠方画像・下方画像の各々に対してパター
ンの同じ箇所を複数箇所抽出し、それぞれの画像の動き
と画面毎の時間経過とから回転・並進速度成分を求めて
フレーム間の並進速度成分を原点（測距開始地点で空間
に固定したＸＹＺ座標系原点）から見た並進速度成分に
変換し、３次元空間で蓄積して航法軌跡を求めることに
より、自己位置をリアルタイムで認識する。

【００５５】上記自己位置認識装置３０の機能は、図１
に示すように、初期化処理部３１、Ｎｏ１ブロック群取
得処理部３２、Ｎｏ２ブロック群取得処理部３３、回転
演算処理部３４、並進演算処理部３５、航法計算処理部
３６、通信処理部３７によって示すことができる。

【００５６】初期化処理部３１では、ハードウエアの初
期化、各定数値、各変数値の初期化を行うと共に、以下
に述べるＮｏ１ブロック群及びＮｏ２ブロック群の初期
データを蓄積する。

【００５７】Ｎｏ１ブロック群取得処理部３２では、遠
方元画像及び下方元画像のそれぞれから、航法演算処理
に適正な画像領域の絵柄箇所（以下、Ｎｏ１ブロックと
称する）を所定の間隔で切り出す処理を行い、複数のＮ
ｏ１ブロック（Ｎｏ１ブロック群）を、遠方画像、下方
画像のそれぞれについて取得する。

【００５８】この場合、上記Ｎｏ１ブロックは適度に小
さい領域とし、画像が多少回転しても平行移動のみで個
々のＮｏ１ブロックに対して以下のＮｏ２ブロックを確
実にマッチング可能なようにし、また、膨大な情報量の
中から測距点までの距離を確実に絞れるようにする。本
形態では、Ｎｏ１ブロックの大きさを１２×６ピクセル
の小領域とする。

【００５９】このＮｏ１ブロック群の取得に際しては、
１２×６ピクセルの領域に対して出現する距離画像値の
ヒストグラムを作成し、このヒストグラム情報によって
撮像画像の信頼性を評価し、切り出し箇所の良否を判定
する。すなわち、距離画像は、左右のカメラが捕らえた
画像パターンが同じならば、走査線方向のズレ量に応じ
た距離画像値が得られるため、この特徴を利用し、同じ
距離画像値が周辺に所定の数があれば、対象画像は実在
する確かな画像であると判断する。

【００６０】Ｎｏ２ブロック群取得処理部３３では、所
定時間後に撮像した画像に対し、Ｎｏ１ブロックの絵柄
と同じ箇所を探索するための探索エリアを設定し、各探
索エリア毎にシティブロック距離を計算してマッチング
具合を判定し、Ｎｏ１ブロックの絵柄と同じ箇所（以
下、Ｎｏ２ブロックと称する）を抽出する。この処理
を、遠方画像、下方画像のそれぞれについて行い、複数
のＮｏ２ブロック（Ｎｏ２ブロック群）を取得する。

【００６１】回転演算処理部３４では、遠方画像（遠方
元画像）のＮｏ１ブロック群からＮｏ２ブロック群への
オプティカルフロー（Ｎｏ１ブロックのイニシャル座標
からＮｏ２ブロックの特定座標までの撮像座標面での動
きを表した移動ベクトルの分布）に基づいて実距離成分
の差を求め、この実距離成分の差から回転速度成分（ロ
ール・ピッチ・ヨーの角速度）を算出する。

【００６２】並進演算処理部３５では、下方画像（下方
元画像）のＮｏ１ブロック群からＮｏ２ブロック群への
オプティカルフローに基づいて実距離成分の差を求め、
この実距離成分の差による速度成分から回転速度成分を
除いて３次元方向（Ｘ，Ｙ，Ｚ方向）の純並進速度を求
める。

【００６３】航法計算処理部３６では、上記並進演算処
理部３５での演算結果を原点から見た３次元方向の速度
に変換し、イニシャル地点から速度成分を累積して原点
からの移動量や現在位置を割り出す。また、通信処理部
３７では、パラレルあるいはシリアル通信によって航法
データを外部のターゲットコントロールシステム１００
に出力する。

【００６４】以上の時系列的なアルゴリズムに対し、実
際には、遠方・下方の画像の取り込みは同時に行う必要
があり、無人移動システムにおけるナビゲーションとし
ての実用性を考慮した場合、短い時間周期（例えば、
０．１ｓｅｃ毎）で航法データを外部システムに送出す
ることが望ましい。

【００６５】従って、上記自己位置認識装置３０は、具
体的には、図３に示すハードウエア構成となっており、
このハードウエア構成によって並列処理による高速処理
を実現している。

【００６６】すなわち、上記自己位置認識装置３０は、
マスタースレーブ関係にあるＮｏ１ブロック群取得処理
用のマスタープロセッサ（ＳＹＳ１マスター）４０及び
通信処理用のスレーブプロセッサ（ＳＹＳ１スレーブ）
４１と、同じくマスタースレーブ関係にあるＮｏ２ブロ
ック群取得処理用のマスタープロセッサ（ＳＹＳ２マス
ター）４２及び回転・並進・航法計算用のスレーブプロ
セッサ（ＳＹＳ２スレーブ）４３とがデュアルポートＲ
ＡＭ（ＤＰＲＡＭ）４４を介して接続され、さらに、シ
ティブロック計算処理用のマイクロプロセッサ４５、ヒ
ストグラム計算処理用のマイクロプロセッサ４６が接続
された基本構成となっている。

【００６７】上記ＳＹＳ１マスター４０側のバスには、
ワークエリア・処理履歴記録用のＤＲＡＭ４７、１２×
６ピクセル領域における距離画像値のヒストグラム作成
結果が記録されるＤＰＲＡＭ４８、及び、Ｎｏ１ブロッ
ク群の探索エリア座標データが記録されるＤＰＲＡＭ４
９が接続されている。

【００６８】一方、上記ＳＹＳ２マスター４２側のバス
には、同様にワークエリア・処理履歴記録用のＤＲＡＭ
５０、Ｎｏ１ブロック元画像のパターンデータが記録さ
れるＤＰＲＡＭ５１、Ｎｏ２ブロックのシティブロック
距離計算結果が記録されるＤＰＲＡＭ５２が接続され、
さらに、上記ステレオ画像処理装置２０の各メモリ２１
〜２４が接続されている。

【００６９】そして、上記ヒストグラム計算処理用マイ
クロプロセッサ４６に、遠方距離画像メモリ２３及び下
方距離画像メモリ２４からのデータが入力され、処理結
果が上記ＤＰＲＡＭ４８に記録されるとともに、上記シ
ティブロック計算処理用マイクロプロセッサ４５に、遠
方元画像メモリ２１及び下方元画像メモリ２２からのデ
ータ、ＤＰＲＡＭ４９，５１からのデータが入力され、
処理結果が上記ＤＰＲＡＭ５２に記録される。

【００７０】尚、図３においては、片方向のデータの流
れを矢印の線によって示し、双方向性のデータの流れは
矢印の無い線によって示す。

【００７１】以上のハードウエア構成による処理の流れ
について概略すると、図４に示すように、まず、遠方用
ステレオカメラ１２で撮像した画像に対し、１２×６ピ
クセルの距離画像評価を経てＮｏ１ブロック群の取得処
理を行い、この処理が終わると、下方用ステレオカメラ
１３で撮像した画像に対し、１２×６ピクセルの距離画
像評価を経てＮｏ１ブロック群の取得処理が開始され
る。

【００７２】同時に、遠方画像に対し、シティブロック
距離（ＣＢＫ）評価によるＮｏ２ブロック群のマッチン
グ処理が開始され、この遠方マッチング処理が完了して
航法処理が始まると、下方画像に対するＣＢＫ評価を経
てＮｏ２ブロック群のマッチング処理が始まる。

【００７３】航法処理では、回転速度計算から並列速度
計算を経て航法軌跡を求める航法計算を行い、航法デー
タが算出されると、通信処理によって外部システムへデ
ータが送信される。そして、この航法データによる自己
位置に基づいて、予め設定された絶対座標の航路あるい
は地上の目標物に対して設定された航路をとるよう、ヘ
リコプターの対地高度や飛行方向が精密に制御される。

【００７４】以下、主として自己位置認識装置３０にお
いて実行される自己位置認識に係わる処理について、図
５〜図１１のフローチャートに従って説明する。

【００７５】図５はＳＹＳ１マスター３１におけるＳＹ
Ｓ１初期化処理ルーチン、図６はＳＹＳ２マスター３３
におけるＳＹＳ２初期化処理ルーチンであり、システム
起動時には、Ｎｏ１ブロック群取得処理用のＳＹＳ１マ
スター３１側のＳＹＳ１初期化処理ルーチンが先に実行
される。

【００７６】図５のＳＹＳ１初期化処理ルーチンでは、
ステップS10でＳＹＳ１側のハードウエア各部を初期化
し、ステップS11でステレオカメラの仕様定数値を初期
化すると、ステップS12で、航法処理変数値や、その他
の変数値を初期化する。次いで、ステップS13へ進み、
アイドル処理としてＮｏ１ブロック群の取得処理を実行
し、Ｎｏ１ブロックを１個取得すると、ステップS14へ
進み、アイドル処理終了か否かを判断する。

【００７７】そして、アイドル処理終了でないときに
は、上記ステップS14からステップS13へ戻ってＮｏ１ブ
ロックの取得を続行し、アイドル処理終了のとき、ＳＹ
Ｓ１初期化処理ルーチンを終了する。このアイドル処理
は、航法処理の開始に際してバッファ内にＮｏ１ブロッ
クのデータを蓄積する処理であり、例えば、バッファが
３個の場合には、ステップS13→ステップS14→ステップ
S13のループを３回繰り返す。

【００７８】以上のＳＹＳ１初期化処理ルーチンでＮｏ
１ブロックが取得されると、図６のＳＹＳ２初期化処理
ルーチンがスタートし、ステップS20で、同様に、ＳＹ
Ｓ２側のハードウエア各部を初期化し、ステップS21で
ステレオカメラの仕様定数値を初期化すると、ステップ
S22で、航法処理変数値や、その他の変数値を初期化す
る。

【００７９】そして、同様に、ステップS23でのＮｏ２
ブロック群の取得処理、ステップS24でのアイドル処理
終了判断を経てステップS23→ステップS24→ステップS2
3の処理ループを所定回数繰り返し、ＳＹＳ２初期化処
理ルーチンを終了する。

【００８０】次に、図７に示すＮｏ１ブロック群の取得
処理について説明する。この処理では、まず、ステップ
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,…,2
2,23,24までの計２５個取得した例を示している。

【００９７】以上により、遠方画像・下方画像のそれぞ
れについてＮｏ１ブロック群を取得すると、Ｎｏ１ブロ
ックと同じ絵柄を持つＮｏ２ブロックを探す処理に移行
する。航法計算の品質を確保するためには、Ｎｏ１ブロ
ックの取得数分ｎ個を評価し、できるだけ多く（最大ｎ
個）のＮｏ２ブロックを探し出す必要がある。ｎ個のＮ
ｏ１・Ｎｏ２ブロックは、それぞれペアの関係であり、
ペアにならなかったＮｏ１ブロックは排除し、新たなＮ
ｏ１・Ｎｏ２ブロック群を求める。

【００９８】以下、図８に示すＮｏ２ブロック群の取得
処理について説明する。この処理では、ステップS40で
Ｎｏ１ブロック取得数の評価を行い、所定の取得数に達
していないときには不合格と判定してルーチンを抜け、
所定の取得数に達して合格判定のときには、ステップS4
1へ進んで遠方・下方処理の判定を行う。

【００９９】そして、遠方画像に対する処理のときに
は、上記ステップ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の処理によってカメラ固
定のＸＹＺ直交座標系における角速度成分を求めると、
次に、ステップ56へ進み、ロール成分ｒｌ、ピッチ成分
ｐｔ、及び、ヨー成分ｙｗを、回転行列Ｒｏｔの要素に
展開してルーチンを抜ける。この回転行列Ｒｏｔは、ロ
ール、ピッチ、ヨーの各軸毎の回転行列の積であり、例
えば、以下の(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→ステップS6
1のループで排除したブロック数が多く、計算精度を維
持するに必要なサンプル数を確保できないときには、前
回の並進速度成分の計算結果を今回処理の値として並進
演算処理を終了する。

【０１２７】続くステップS63では、Ｎｏ１・Ｎｏ２ブ
ロックの撮像面座標値と、それぞれの距離画像値とから
実空間の座標（カメラ固定のＸＹＺ直交座標）値を求
め、ステップS64で、Ｎｏ２ブロックから求めた実空間
座標値を、最新の回転行列で回転成分を差し引いた純並
進成分の実空間座標値に変換し、ステップS65へ進ん
で、画像の動き（移動ベクトル）を実空間座標値の差分
で求める。

【０１２８】すなわち、遠方画像の撮像面と下方画像の
撮像面とは互いに直交関係にあることから、回転移動と
並進移動とを含んだＮｏ１・Ｎｏ２ブロック間の移動ベ
クトル量に、先に求めた回転行列Ｒｏｔ（Ｎｏ２→Ｎｏ
１ブロックへの回転行列）を掛け合わせることにより
（相対的に回転させることにより）、回転成分の動きを
実距離のクラスで除去することができる。

【０１２９】次いで、ステップS66へ進み、実空間移動
量を新たに記録して測距したブロック数を勘定すると、
ステップS67で、Ｎｏ２ブロック群の全てを評価したか
否かを調べる。そして、全部の評価が終わっていないと
きには、ステップS61へ戻り、全部の評価が終わったと
き、ステップS68へ進み、区間推定等の統計処理によっ
てオプティカルフロー全体に対して方向や大きさが異常
である移動ベクトル（特異ベクトル）を排除するフィル
タ処理を行い、特異ベクトルを排除した後の平均移動量
を算出する。

【０１３０】その後、ステップS69で、特異ベクトルの
除去結果を調べ、特異ベクトルの数が多く、航法計算の
精度を維持するのに必要なデータ数が確保できなくなっ
たときには、前述のステップS71で前回の並進速度成分
の計算結果を今回処理の値としてメモリを更新してルー
チンを抜ける。一方、特異ベクトルが殆ど無いか、ある
いは航法計算の精度を損なわない程度の数であるときに
は、上記ステップS69からステップS70へ進んで、成分
（Ｘｌ,Ｙｌ,Ｚｌ）の３次元並進速度｜Ｓ｜を求め、こ
の新しい並進速度でメモリ値を更新してルーチンを抜け
る。

【０１３１】以上の並進演算処理に続いて航法軌跡を求
める図１１の航法演算処理が実行される。この航法演算
処理では、まず、ステップS80で原点（測距開始地点）
から見た移動体の姿勢状態を求める。原点から見た移動
体の姿勢状態は、測距開始地点で空間に固定したＸＹＺ
直交座標系から見た回転行列Ｒｏｔｓによって示され
る。このため、まず、測距開始地点で回転行列を単位行
列化して初期化することで撮像面に直交する平面を実空
間を表す基準面とし、この初期化時点の行列にフレーム
間の回転行列（カメラ固定のＸＹＺ直交座標系から見た
回転行列）Ｒｏｔを右から掛け込んだ行列を原点から見
た移動体の姿勢を表す回転行列Ｒｏｔｓとする。

【０１３２】すなわち、原点から見た姿勢を表す回転行
列Ｒｏｔｓは、カメラ固定の座標系から見たフレーム毎
の回転行列Ｒｏｔを時系列的に積算した正方行列であ
り、以下の(13)式に示すように、前回までの原点から見
た姿勢状態を表す回転行列Ｒｏｔｓに今回の回転演算処
理で求めたフレーム間の回転行列Ｒｏｔを右から積算し
た正方行列で今回処理における原点から見た姿勢を表す
回転行列Ｒｏｔｓとして更新することにより、原点から
見た移動体の最新の姿勢を表すことができる。

【０１３３】Ｒｏｔｓ＝Ｒｏｔｓ・Ｒｏｔ …(13) 尚、上記回転行列Ｒｏｔｓを用いることにより、以下の
(14)〜(16)式によって原点から見た角速度成分Ｒｏｌｌ
（ロール），Ｐｉｃｔｈ（ピッチ），Ｙｏｗ（ヨー）を
求めることができる。

【０１３４】Ｒｏｌｌ＝ａｔａｎ(Ｒｏｔｓ[0][1]／Ｒｏｔｓ[0][0]) …(14) Ｐｉｔｃｈ＝ａｔａｎ(Ｒｏｔｓ[1][2]／Ｒｏｔｓ[2][2]) …(15) Ｙｏｗ＝ａｔａｎ(Ｒｏｔｓ[2][0]／Ｒｏｔｓ[2][2]) …(16) 次に、ステップS81へ進み、フレーム間の並進速度成分
を原点から見た並進速度成分に変換すると、ステップS8
2で、原点から見た速度成分をフレーム毎に累積し、イ
ニシャル地点からの移動体の実空間座標を求める。

【０１３５】原点から見た並進速度Ｓｏは、原点から見
た姿勢を表す回転行列Ｒｏｔｓとフレーム間の３次元並
進速度Ｓとの積を、以下の(17)式に示すように、前回ま
での原点から見た並進速度Ｓｏに累積することで求める
ことができ、累積したベクトル量に所定の距離換算値を
掛け合わせることで、図２３に示すような、航法軌跡を
得ることができ、原点からの移動体の移動量を知ること
ができる。Ｓｏ＝Ｓｏ＋(Ｒｏｔｓ)・Ｓ …(17)

【０１３６】この場合、地上の適当な地点に、図２２に
示すようなランドマークを設置しておき、このランドマ
ークをパターン認識等によって識別し、測距開始地点と
して原点をイニシャルすることで、上記航法軌跡から自
己の相対位置を認識することができる。

【０１３７】さらには、ＧＰＳ等の衛星測位システムか
らの測位情報や既知の地図情報によって地上の適当な地
点を原点としてイニシャルすることで、航法軌跡から自
己の絶対位置を認識することができる。

【０１３８】以上により、撮像対象物までの距離を細か
く認識して、極めて正確な測量を行うことができ、複雑
な地形での低空飛行においても高精度な対地高度が得ら
れる。すなわち、従来に比較してドリフトの少ない高精
度の自律航法機能を担うことが可能であり、膨大な情報
量を有する画像ならではの周囲環境分析を航法処理に反
映することができる。

【０１３９】

【発明の効果】以上説明したように本発明によれば、一
定時間毎に遠方風景と下方風景とを撮像し、それぞれの
撮像画像の中から特徴的な絵柄の領域を第１のブロック
として複数箇所抽出し、次のタイミングで撮像した画像
の中から第１のブロックと同じ絵柄を有する領域を第２
のブロックとして探索すると、遠方風景の撮像画像にお
ける第１のブロックから第２のブロックへの画像の動き
とフレーム間の時間経過とから、移動体のフレーム間の
回転速度成分を求め、また、下方風景の撮像画像におけ
る第１のブロックから第２のブロックへの画像の動きと
フレーム間の時間経過とから、移動体のフレーム間の速
度成分を求め、この速度成分から上記回転速度成分を除
去して移動体のフレーム間の並進速度成分を求める。そ
して、このフレーム間の並進速度成分を測距開始地点か
ら見た並進速度成分に変換し、この変換した並進速度成
分を累積して３次元空間における航法軌跡を求め、移動
体の自己位置を認識するため、ドリフトの少ない高精度
の自律航法機能を担うことができ、膨大な情報量を有す
る画像ならではの周囲環境分析を航法処理に反映するこ
とができる等優れた効果が得られる。

【図面の簡単な説明】

【図１】自己位置認識装置の機能構成図

【図２】３次元空間計測用カメラアセンブリの構成図

【図３】自己位置認識装置の回路ブロック図

【図４】並列処理のタイムチャート

【図５】ＳＹＳ１初期化処理ルーチンのフローチャート

【図６】ＳＹＳ２初期化処理ルーチンのフローチャート

【図７】Ｎｏ１ブロック群取得処理ルーチンのフローチ
ャート

【図８】Ｎｏ２ブロック群取得処理ルーチンのフローチ
ャート

【図９】回転演算処理ルーチンのフローチャート

【図１０】並進演算処理ルーチンのフローチャート

【図１１】航法演算処理ルーチンのフローチャート

【図１２】遠方のＮｏ１ブロックの取得パターンを示す
説明図

【図１３】画像評価におけるヒストグラム分布の好まし
くない例を示す説明図

【図１４】画像評価におけるヒストグラム分布の好まし
い例を示す説明図

【図１５】下方のＮｏ１ブロックの取得パターンを示す
説明図

【図１６】探索エリアの設定を示す説明図

【図１７】遠方画像の特徴を示す説明図

【図１８】遠方画像に探索エリアを設定した例を示す説
明図

【図１９】下方の撮像面上の動きを示す説明図

【図２０】下方画像に探索エリアを設定した例を示す説
明図

【図２１】三角測量による撮像面座標から実距離への換
算を示す説明図

【図２２】測距開始地点のランドマークを示す説明図

【図２３】自律航法による航法軌跡の説明図

【符号の説明】

１０…カメラアセンブリ２０…ステレオ画像処理装置３０…自己位置認識装置

フロントページの続き (51)Int.Cl.⁶ 識別記号ＦＩＧ０１Ｃ 21/20 Ｇ０１Ｃ 21/20 Ｇ０６Ｔ 7/00 Ｇ０６Ｆ 15/62 ４１５

Claims

【特許請求の範囲】

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