JP4316936B2 - アクティブセンサの動物体検出装置及び動物体検出方法、並びに動物体検出プログラム - Google Patents
アクティブセンサの動物体検出装置及び動物体検出方法、並びに動物体検出プログラム Download PDFInfo
- Publication number
- JP4316936B2 JP4316936B2 JP2003162834A JP2003162834A JP4316936B2 JP 4316936 B2 JP4316936 B2 JP 4316936B2 JP 2003162834 A JP2003162834 A JP 2003162834A JP 2003162834 A JP2003162834 A JP 2003162834A JP 4316936 B2 JP4316936 B2 JP 4316936B2
- Authority
- JP
- Japan
- Prior art keywords
- movement
- distance image
- pixel
- estimated
- occlusion
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Length Measuring Devices By Optical Means (AREA)
- Image Processing (AREA)
- Image Analysis (AREA)
Description
【０００１】
【発明の属する技術分野】
本発明は、アクティブセンサの動物体検出装置及び動物体検出方法、並びに動物体検出プログラムに関するものである。
【０００２】
【従来の技術】
近年、人間と共存する移動ロボットの開発が盛んに行われている。ロボットが自律移動するためには、外界情報を獲得し環境を把握する必要がある。環境を認識する上で有用なセンサとして視覚センサが挙げられる。これまで多くの移動体ビジョンの研究は、静止環境下で移動するためのものである。しかし、人間と共存するロボットを実現するためには、人物等の動物体が存在する動的環境下でも衝突を回避し、安全に移動することや、人物を追跡するため、移動体自身が移動しつつ動物体を検出することが求められている（非特許文献１〜３）。
【０００３】
観測系が移動する場合、取得される画像には、観測系の移動（視点移動）による「見かけの変化」と、人物等の「動物体による環境自身の変化」が含まれる。動物体を検出するとは、「環境自身の変化」のみを抽出することであり、この２つの変化を区別する必要がある。観測系の移動による「見かけの変化」には、静止環境の相対運動による変化と、オクルージョンによる変化がある。視点移動に伴う静止環境の相対運動による変化は、画像においては静止環境の位置や大きさの変化として表れる。オクルージョンによる変化とは、移動前には見えていない領域が視点が移動することにより見えたり、見えていた領域が見えなくなったりする等の見かけの変化である。
【０００４】
このオクルージョンは、特にカメラに対して物体が近い場合、領域として大きく現れ、人物等の動物体による環境変化と区別することが困難になる。ロボットが自律移動を行うためには、ロボットの近くの情報、変化の検出が重要となる。
【０００５】
従来、観測系の運動を考慮し、動物体を検出する方法としてオプティカルフローを用いて推定する方法が提案されている（非特許文献４〜７）。これらは、静止物体の領域のオプティカルフローが、消失点を中心として放射状に分布する性質を利用して、その性質を満足しない領域を動物体として検出している。
【０００６】
【非特許文献１】
石川繁樹，浅香俊一，“移動障害物を含むような動的変化を伴う走行環境における自律移動ロボットの走行誘導方式”，日本ロボット学会誌学術論文，Vol.11,No.6,pp.856-867,1993.
【非特許文献２】
井上晃，井上健司，大川善邦，“複数移動障害物の行動予測に基づく自律移動ロボットのオンライン回避行動”，日本ロボット学会誌学術論文，Vol.15,No.2,pp.249-260,1997.
【非特許文献３】
松村幸輝，村井保之，“遺伝的プログラミングに基づくファジーロボットの障害物回避”，電子情報通信学会誌学術論文誌 A，Vol.J83-A,No.12,pp.1539-1551,2000.
【非特許文献４】
太田直哉，“信頼性情報をもったオプティカルフローからの形状復元とその移動物体検出への応用”，電子情報通信学会誌学術論文誌(D-II), Vol.J76-D-II,No.8,pp.1562-1571,1993.
【非特許文献５】
武田信之，渡辺睦，小野口一則，“消失点推定残差法を用いた移動障害物検出”，第１回ロボティクスシンポジア予稿集，pp.29-34,1996.
【非特許文献６】
海老根巧，浜田望，“観測系の運動を考慮したオプティカルフロー推定に基づく運動体検出”，電子情報通信学会誌学術論文誌(D-II), Vol.J83-D-II,No.6,pp.1498-1506,2000.
【非特許文献７】
岡田隆三，白井良明，三浦純，久野義徳“オプティカルフローと距離情報に基づく動物体追跡”，電子情報通信学会誌学術論文誌(D-II), Vol.J80-D-II,No.6,pp.1530-1538,1997.
【０００７】
【発明が解決しようとする課題】
しかし、これらのオプティカルフローを用いた方法では、原理的に光軸に平行な運動の時がフローベクトルが小さいため、背景と動物体とを区別することが難しいという問題がある。又、急激な見かけの変化が生じることのないよう、時間移動量が小さいと仮定しているが、例えば、カメラと動物体までの距離が近い場合では、少しの移動でも見かけの変化が大きくなり、動物体を正確に抽出することが困難である。
【０００８】
本発明は、観測系の移動による動物体以外の環境の見かけの変化と、動物体の移動による環境変化とを容易に区別して動物体の検出が容易にできるアクティブセンサの動物体検出方法及び動物体検出装置を提供することを目的としている。又、オクルージョン領域を推定することにより、より正確に動物体を検出することができるアクティブセンサの動物体検出方法及び動物体検出装置を提供することを目的としている。
【０００９】
又、上記アクティブセンサの動物体検出方法及び動物体検出装置を容易に実現できる動物体検出プログラムを提供することを目的としている。
【００１０】
【課題を解決するための手段】
上記問題点を解決するため、請求項１記載の発明は、全方位距離画像を取得可能であり、環境内を移動可能なアクティブセンサの動物体検出装置において、前記アクティブセンサの移動前と移動後の、移動方向、移動距離、回転量を含む相対移動量を推定する相対移動量推定手段と、移動前に取得した全方位距離画像と、前記相対移動量推定手段にて推定した相対移動量に基づいて、移動後の全方位距離画像(以下、予測距離画像という)を推定して生成する予測距離画像生成手段と、前記アクティブセンサが移動することにより、前記予測距離画像に生ずるオクルージョン領域を推定するオクルージョン領域推定手段と、前記予測距離画像と、移動後にアクティブセンサが取得した全方位距離画像との差分画像を生成する際に、前記オクルージョン領域を取り除き、動物体領域を抽出する動物体領域抽出手段とを備えたことを特徴とするアクティブセンサの動物体検出装置を要旨とするものである。
【００１１】
請求項２の発明は、請求項１において、前記オクルージョン領域推定手段は、移動前に取得した全方位距離画像のジャンプエッジの画素(以下、ジャンプエッジ画素という)と、同画素の所定範囲内に位置する画素の中で、アクティブセンサから所定値以上の距離値を有する画素(以下、対比対象画素という)について、前記相対移動量に基づき、それぞれ移動後の推定方位角、及び推定仰角を求め、前記ジャンプエッジ画素と対比対象画素のそれぞれの推定方位角の区間、及び推定仰角の範囲を、前記予測距離画像における前記オクルージョン領域のオクルージョン区間であると推定することを特徴とする。
【００１２】
請求項３の発明は、請求項２において、前記オクルージョン領域推定手段は、前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の仰角が、前記ジャンプエッジ画素の推定仰角に一致し、かつ、前記判定対象画素の方位角が前記推定方位角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定することを特徴とする。
【００１３】
請求項４の発明は、請求項２において、前記オクルージョン領域推定手段は、前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の方位角が、前記ジャンプエッジ画素の推定方位角に一致し、かつ、前記判定対象画素の仰角が前記推定仰角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定することを特徴とする。
【００１４】
請求項５の発明は、請求項１乃至請求項４のうちいずれか１項において、前記動物体領域抽出手段は、前記差分画像を生成する際に、各画素の距離値の差に基づいて、正、負のうち、少なくともいずれか一方の符号判定を行い、この符号判定に基づいて、動物体領域抽出を行うことを特徴とする。
【００１５】
請求項６の発明は、全方位距離画像を取得可能であり、環境内を移動可能なアクティブセンサの動物体検出方法において、前記アクティブセンサの移動前と移動後の、移動方向、移動距離、回転量を含む相対移動量を推定する第１ステップと、移動前に取得した全方位距離画像と、前記第１ステップにて推定した相対移動量に基づいて、移動後の全方位距離画像(以下、予測距離画像という)を推定して生成する第２ステップと、前記アクティブセンサが移動することにより、前記予測距離画像に生ずるオクルージョン領域を推定する第３ステップと、前記予測距離画像と、移動後にアクティブセンサが取得した全方位距離画像との差分画像を生成する際に、前記オクルージョン領域を取り除き、動物体領域を抽出する第４ステップを含むことを特徴とするアクティブセンサの動物体検出方法を要旨とするものである。
【００１６】
請求項７の発明は、請求項６において、前記第３ステップは、移動前に取得した全方位距離画像のジャンプエッジの画素(以下、ジャンプエッジ画素という)と、同画素の所定範囲内に位置する画素の中で、アクティブセンサから所定値以上の距離値を有する画素(以下、対比対象画素という)について、前記相対移動量に基づき、それぞれ移動後の推定方位角、及び推定仰角を求め、前記ジャンプエッジ画素と対比対象画素のそれぞれの推定方位角の区間、及び推定仰角の範囲を、前記予測距離画像における前記オクルージョン領域のオクルージョン区間であると推定することを含むことを特徴とする。
【００１７】
請求項８の発明は、請求項７において、前記第３ステップは、前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の仰角が、前記ジャンプエッジ画素の推定仰角に一致し、かつ、前記判定対象画素の方位角が前記推定方位角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定することを特徴とする。
【００１８】
請求項９の発明は、請求項７において、前記第３ステップは、前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の方位角が、前記ジャンプエッジ画素の推定方位角に一致し、かつ、前記判定対象画素の仰角が前記推定仰角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定することを特徴とする。
【００１９】
請求項１０の発明は、請求項６乃至請求項９のうちいずれか１項において、前記第４ステップは、前記差分画像を生成する際に、各画素の距離値の差に基づいて、正、負のうち、少なくともいずれか一方の符号判定を行い、この符号判定に基づいて、動物体領域抽出を行うことを特徴とする。
【００２０】
請求項１１の発明は、コンピュータを、アクティブセンサの移動前と移動後の、移動方向、移動距離、回転量を含む相対移動量を推定する相対移動量推定手段と、移動前に取得した全方位距離画像と、前記相対移動量推定手段にて推定した相対移動量に基づいて、移動後の全方位距離画像(以下、予測距離画像という)を推定して生成する予測距離画像生成手段と、前記アクティブセンサが移動することにより、前記予測距離画像に生ずるオクルージョン領域を推定するオクルージョン領域推定手段と、前記予測距離画像と、移動後にアクティブセンサが取得した全方位距離画像との差分画像を生成する際に、前記オクルージョン領域を取り除き、動物体領域を抽出する動物体領域抽出手段として、機能させることを特徴とするアクティブセンサの動物体検出プログラムを要旨とするものである。
【００２１】
請求項１２の発明は、請求項１１において、さらに、コンピュータを、前記オクルージョン領域推定手段として、移動前に取得した全方位距離画像のジャンプエッジの画素(以下、ジャンプエッジ画素という)と、同画素の所定範囲内に位置する画素の中で、アクティブセンサから所定値以上の距離値を有する画素(以下、対比対象画素という)について、前記相対移動量に基づき、それぞれ移動後の推定方位角、及び推定仰角を求め、前記ジャンプエッジ画素と対比対象画素のそれぞれの推定方位角の区間、及び推定仰角の範囲を、前記予測距離画像における前記オクルージョン領域のオクルージョン区間であると推定するように機能させることを特徴とする。
【００２２】
請求項１３の発明は、請求項１２において、さらに、コンピュータを、前記オクルージョン領域推定手段として、前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の仰角が、前記ジャンプエッジ画素の推定仰角に一致し、かつ、前記判定対象画素の方位角が前記推定方位角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定するように機能させることを特徴とする。
【００２３】
請求項１４の発明は、請求項１２において、さらに、コンピュータを、前記オクルージョン領域推定手段として、前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の方位角が、前記ジャンプエッジ画素の推定方位角に一致し、かつ、前記判定対象画素の仰角が前記推定仰角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定するように機能させることを特徴とする。
【００２４】
請求項１５の発明は、請求項１１乃至請求項１４のうちいずれか１項において、
さらに、コンピュータを前記動物体領域抽出手段として、前記差分画像を生成する際に、各画素の距離値の差に基づいて、正、負のうち、少なくともいずれか一方の符号判定を行い、この符号判定に基づいて、動物体領域抽出を行うことを特徴とする。
【００２５】
【発明の実施の形態】
以下、本発明のアクティブセンサの動物体検出装置を具体化した一実施形態を図１〜図２３を参照して説明する。
【００２６】
本実施形態のアクティブセンサの動物体検出装置は、アクティブセンサ１１とコンピュータ１６にて構成されている。
図１はアクティブセンサの電気的構成を示すブロック図である。図２はアクティブセンサ１１の機械的構成の概略図である。
【００２７】
アクティブセンサ１１は、複数の車輪を有する車体βを備えており、車体β内に設けられた図示しない電動モータにより前記車輪が駆動されて環境Ｋ内の任意の位置に向けて自動走行（直線及び曲線に沿った走行を含む）及びが可能である。なお、図２においては、説明の便宜上、環境Ｋは、車体βに比較して、小さく図示している。
【００２８】
アクティブセンサ１１は、複数の３眼ステレオユニット（以下、単にステレオユニット１２という）、メモリユニット１５、同期信号発生器１７等を備えている。メモリユニット１５、コンピュータ１６、同期信号発生器１７等は車体β内に格納されている。
【００２９】
前記複数のステレオユニット１２は、撮像手段に相当する。又、前記コンピュータ１６は、相対移動量推定手段、予測距離画像生成手段、オクルージョン領域推定手段、及び動物体領域抽出手段に相当する。
【００３０】
アクティブセンサ１１は、３次元空間内の、全ての方向(以下、全方位という)におけるカラー画像と３次元情報(距離画像)を、同時刻にリアルタイムで取得することが可能な装置である。ステレオユニット１２は、３個のビデオカメラからなり、各ステレオユニット１２は正２０面体の各面上にそれぞれ配置されている。そして、各ステレオユニット１２は同特性を備え、各面に配置したステレオユニット１２により、全方位のカラー画像と全方位の白黒画像（以下、距離データという）を同一時刻にリアルタイムで取得可能なシステムである。これによって、３次元空間上の全ての方向におけるカラー画像と３次元情報を同一時刻に得ることができる。
【００３１】
又、同じ特性を持つステレオユニット１２を正２０面体の各面上に配置したことにより、３次元空間を均等に分割し、高解像度の情報の取得が可能である。なお、このアクティブセンサ１１は、「”実環境センシングのための全方位ステレオシステム（ＳＯＳ）”、電気学会論文誌C.Vol.121-C,No.5,pp.876-881,2001」に記載されている。
【００３２】
ステレオユニット１２では、図１に示すように、１つの基準ビデオカメラＶＣｓと、一対の参照用ビデオカメラＶＣとからなる。そして、参照用ビデオカメラＶＣは基準ビデオカメラＶＣｓの光軸を交線として、互いに直交する一対の平面に含ませるように配置されている。そして、これらのカメラにより、２つのステレオペアを構成するように配置されている。
【００３３】
各ステレオユニット１２からは、１枚のカラー画像と２枚の白黒画像とからなるステレオ画像が取得され、全方位の２０枚のカラー画像と４０枚の白黒画像を１セットとして１５セット／秒でメモリユニット１５に転送する。メモリユニット１５は、転送されてきた全方位の画像データを記憶する。
【００３４】
各ステレオユニット１２の各ビデオカメラには、同期信号発生器１７から共通の外部同期信号が供給されている。このことにより、ディジタル化されたフレームにおいて、完全に同期した画像データが得られる。
【００３５】
コンピュータ１６は、所定の周期毎に、同コンピュータ１６内に備えているＲＯＭ１６ａに予め格納したアクティブセンサの動物体検出プログラムを実行する。前記ＲＯＭ１６ａを備えたコンピュータ１６は記憶手段に相当する。
【００３６】
又、コンピュータ１６は、メモリユニット１５にアクセスし、全方位のカラー画像と距離データをその時々に取得する。
（作用）
さて、以上のように構成されたアクティブセンサの動物体検出装置の作用を図３〜図２３を参照して説明する。
【００３７】
図３（ａ）は、コンピュータ１６が実行するアクティブセンサの動物体検出プログラムのフローチャートであり、コンピュータ１６は、所定周期毎に実行し、アクティブセンサ１１の動物体検出を行う。
【００３８】
以下、各ステップ毎に説明する。
１．Ｓ１０（自己位置・姿勢推定）
Ｓ１０では、アクティブセンサ１１の自己位置・姿勢推定を行う。具体的には図３（ｂ）のＳ１１０〜Ｓ１６０の処理を行う。
【００３９】
１．１． Ｓ１１０（エッジの検出）
Ｓ１１０では、エッジ検出を行う。
すなわち、基準ビデオカメラＶＣｓ（センタカメラ）が取得したカラー画像を濃淡化してＬｏＧ（Laplasian of Gaussian）フィルタを通し、ゼロクロス点をエッジとして検出する。
【００４０】
１．２． Ｓ１２０（最大エッジ勾配の算出）
次に、最大エッジ勾配の算出を行う。
すなわち、Sobelフィルタにて各エッジ画素におけるエッジの勾配（強度）を計算し、画像中の最大エッジ勾配（すなわち、最大エッジ強度）を求める。
【００４１】
これは、ＬｏＧフィルタはノイズに影響されやすいため、ノイズや照明の影響で偽のエッジ多く存在するためであり、これを除くための処理である。
この処理によって、強度が最大エッジ勾配値の５％未満のエッジ画素をノイズとしてエッジから削除する。
【００４２】
１．３． Ｓ１３０（現在位置のエッジヒストグラムの生成）
次に、Ｓ１３０において、各ステレオユニット１２のセンタカメラである基準ビデオカメラＶＣｓで得られたエッジ点から、３６０°のパノラマエッジ画像座標系に写像し、縦方向投影のエッジヒストグラムを生成する。エッジヒストグラムは統計量に相当する。
【００４３】
前記３６０°のパノラマエッジ画像は３６０°の円筒画像ともいう。すなわち、この円筒画像におけるエッジを縦方向（円筒の軸心に沿った方向）に投影して、全方位エッジヒストグラムを生成するのである。
【００４４】
図５は、生成された全方位エッジヒストグラムの例を示している。図５において、横軸は０〜３６０°の範囲を示し、縦軸はそのヒストグラムである。図４は、図５に対応するパノラマエッジ画像を示している。
【００４５】
（位置・姿勢と全方位エッジヒストグラムとの関係）
ここで、アクティブセンサ１１の位置・姿勢とヒストグラムとの関係を説明する。
【００４６】
アクティブセンサ１１の移動や回転は、アクティブセンサ１１で得られた全方位の画像のエッジヒストグラムのシフトを引き起こす。
アクティブセンサ１１の回転によって生じたエッジヒストグラムの回転量はすべての方位角において一定である。しかし、アクティブセンサ１１の平行移動で生じたエッジヒストグラムの移動量は移動方向とエッジの方位角に関係する。
【００４７】
図６に示すように、アクティブセンサ１１が移動前地点αからある移動方向ωに向いて動いたとき、ω±ｎπ，ｎ＝０，１の方位角においてはエッジヒストグラムの移動量が小さい。ｎ＝０は移動方向ωであり、ｎ＝１は移動方向ωとは反対方向の意味である。
【００４８】
なお、総合シフト量とは、前記回転量と移動量の合計の量である。又、移動方向ωは、図６に示すように移動前地点αを中心として所定の方向の向きを０°としたとき、その０°からω°離れた方向をいう。
【００４９】
ω±（２ｎ＋１）π／２の方位角においてはそのエッジヒストグラムの移動量が大きくなる。
アクティブセンサ１１が移動方向ωに沿う移動距離をＬとし、移動しながら回転角度φにて回転したとする。
【００５０】
この場合、ある方位角θに位置するエッジピクセルの円筒座標系（３６０°のパノラマエッジ画像座標系）における総合シフト量δθは次の式(１)で決まる。
【００５１】
【数１】
ここで、ｄθは方位角θに位置するエッジの３次元での奥行き（アクティブセンサ１１中心までの距離）である。実際には、多くの場合同じ方位にあるエッジは異なる奥行きをもっているため、式（１）は理想的な場合（同じ方位のエッジは同一の奥行きを持つ場合）だけを表している。
【００５２】
式（１）は、エッジの奥行きに影響されるが、図７（ｃ）に示すように、sinで近似することができ、sin曲線と同様に２πの周期を持つ。その総合シフト量δθの符号はπ周期毎に反転する。
【００５３】
なお、図７（ｃ）は全方位エッジヒストグラムの総合シフト量δθを式（１）で計算した場合の曲線とsin曲線を表した説明図である。
同図において、Ａはsin曲線（正弦波曲線）であり、Ｂは、円筒形の部屋でアクティブセンサ１１が動いた場合の、全方位エッジヒストグラムの総合シフト量δθを式（１）に基づいて演算した曲線である（図７（ａ）参照）。
【００５４】
又、Ｃは正方形の部屋でアクティブセンサ１１が動いた場合の、全方位エッジヒストグラムの総合シフト量δθを式（１）に基づいて演算した曲線である（図７（ｂ）参照）。
【００５５】
本実施形態では、式（１）の符号がπ周期毎に反転する特徴を利用して、全方位エッジヒストグラムの総合シフト量δθからアクティブセンサ１１の移動方向ωと回転角度φを簡単かつロバストに推定するのである。
【００５６】
１．４． Ｓ１４０（動的計画法によるマッチング：ＤＰマッチング）
Ｓ１４０では、アクティブセンサ１１の現在位置(移動後地点)の全方位エッジヒストグラムと移動前地点の全方位エッジヒストグラムをＤＰマッチングし、それぞれの方位角（各移動前地点の方位角）におけるヒストグラムの総合シフト量δθを求める。
【００５７】
移動後地点の全方位エッジヒストグラムは現在位置ヒストグラムに相当し、移動前地点の全方位エッジヒストグラムは移動前地点ヒストグラムに相当する。
この総合シフト量δθを求めることにより、移動前地点に対するアクティブセンサ１１の移動後地点の移動方向ωと回転角度φを算出する。
【００５８】
以下、移動前地点に対する、アクティブセンサ１１の移動後地点の全方位エッジヒストグラムの総合シフト量δθの算出の仕方を詳細に説明する。
（動的計画法マッチング（ＤＰマッチング）について）
ここでは、移動後地点の全方位エッジヒストグラムと、ある移動前地点の全方位エッジヒストグラムのＤＰマッチングについて、すなわち、２つの全方位エッジヒストグラムをＤＰマッチングする方法を説明する。
【００５９】
移動前地点と移動後地点の全方位エッジヒストグラムのそれぞれを
移動前地点：Ｈｐ＝[hp(i),i=0,…,…,N-1]
と
移動後地点：Ｈｃ＝[hc(j),j=0,…,…,N-1｝
とする。
【００６０】
Ｎは３６０の倍数であり、エッジヒストグラムを生成するときの方位角θで決まる。本実施形態では、Ｎ＝７２０である。すなわち、エッジヒストグラムの角度分解能は０．５°である。移動前地点の全方位エッジヒストグラムＨｐの１つのピンhp(i)は、移動と回転によってアクティブセンサ１１の移動後地点の全方位エッジヒストグラムＨｃにおいてシフト量siが生じたとき、hp(i)とhc(i+si)と似ていると仮定することができる。
【００６１】
hp(i)とhc(i+si)との差の二乗をhp(i)とhc(i+si)との間のマッチングコストとすると、次のマッチングコストマトリクスＣ（s,i）が得られる。
【００６２】
【数２】
ここで全方位エッジヒストグラムが２πの周期を持つため、i+si≧Nの場合、
hc(i+si)≡hc(i+si−N)とする。
【００６３】
[(hp(i),hc(i+si)),i=0,…,N-1]が正しいマッチングである場合、それらのマッチングペアはマッチングコストマトリクスＣ（s,i）の中でコストが低く、かつ式（１）の形をした曲線をなす。ここでは、(hp(i),hc(i+si))はhp(i)とhc(i+si)とのマッチングペアを表している。
【００６４】
図８は、アクティブセンサ１１の２つの地点における全方位エッジヒストグラムのマッチングコストマトリクスＣ（s,i）を示す。なお、説明の便宜上、マトリクス中のマッチングコストの低いパスをセンタリングし、上下をカットしている。又、図中、「.-+*&%#$@ABCD」の各記号は、コストのレベルをそれぞれ示し、「.-+*&%#$@ABCD」の順番は、左から右に向かってコストの低い順から高い順に並べている。すなわち、マッチングコストは「.」＜「-」＜「+」＜「*」＜「&」＜「%」＜「#」＜「$」＜「@」＜「A」＜「B」＜「C」＜「D」の大小関係となっている。
【００６５】
図中、縦軸はエッジのシフト量si、横軸はiであり、方位角θに相当する。又、図中、Ｃ（s,i）の中のsin曲線に似たコストの低い曲線は各方位角θにおけるエッジヒストグラムの総合シフト量δθに対応する。Ｃ（s,i）の中のsin曲線に似たコストの低い曲線を探索することで、ＨｐとＨｃの間の全方位ヒストグラムの総合シフト量δθが求められる。
【００６６】
本実施形態では、計算コストの低い動的計画法（ＤＰ）を用いて、Ｃ（s,i）から周期２πを持ち、かつ連続した最小コストのパスを求め、そのパスからロバストに回転角度φと移動方向ωを推定する。
【００６７】
Ｃ（s,i）から周期２πを持ちかつ連続した最小コストのパスは次の条件付き最小化問題に定義することができる。
【００６８】
【数３】
ここで、siは求めたいエッジヒストグラムの総合シフト量である。
【００６９】
hp(i)とhc(j)の周期がNであるため、Ｃ（s,i）のインデックスのsとiに関しては、s±Ｎ→sとi±Ｎ→iで計算される。
パスが特定の行ｋから始まる（すなわち、ｓ０＝ｋ）と仮定した場合には、式（２）の最小化は次のように動的計画法で求めることができる。
【００７０】
【数４】
【００７１】
【数５】
【００７２】
【数６】
Ｓ（s,i）を計算するときは、min[S(s-1,i-1),Ｓ(s,i-1),S(s+1,i-1)]の中のいずれが最小になっていたかを記憶しておき、Ｃmin(k)まで来たパスを逆に辿れば、最小コストのパス（以下、最小コストパスという）が得られる。
【００７３】
ｋ＝０，…，Ｎ−１に対して、上記のようにＣmin(k)を計算し、そのうち、
最小値Ｃ^min(k)＝Ｃmin(k^)
を求める。
【００７４】
Ｃmin(k^)が対応しているパスがＨｐとＨｃの間の最適マッチングとする。
以下、上記パスを最適マッチングパスという。
上記計算により、２πの周期を持ち、かつ連続性のあるエッジヒストグラムのマッチングパスを探索できる。
【００７５】
そして、以下、移動後地点の全方位エッジヒストグラムと他の移動前地点の全方位エッジヒストグラムのマッチングについても同様に処理する。
１．５． Ｓ１５０（姿勢と移動方向の推定）
Ｓ１５０では、Ｓ１４０で得られたアクティブセンサ１１の移動後地点と移動前地点の全方位エッジヒストグラムの総合シフト量から移動前地点に対するアクティブセンサ１１の移動方向ωと回転角度φを推定する。
【００７６】
（回転角度φの推定）
まず、アクティブセンサ１１の回転角度φの推定について説明する。
ＤＰマッチングで得られた最小コストパスであるsi,(i=0,…,N-1)は、アクティブセンサ１１の移動後地点と移動前地点のそれぞれの全方位エッジヒストグラムＨｃとＨｐの間の総合シフト量を表している。図９中のsin曲線の近傍に示された波形はＤＰマッチングで得られた最小コストパスを示す。
【００７７】
式（１）で示したように、これらの総合シフト量はアクティブセンサ１１の回転角度φで生じたヒストグラム全体の回転量と、移動方向ωにおける平行移動で生じた各方位角での移動量からなる。
【００７８】
式（１）から分かるように、ヒストグラムの総合シフト量は回転角度φに相当する回転量ｓφを引けば、引いた後のシフト量はπ周期毎に反転する。すなわち、回転角度φを中心にπ周期で、上下反転する（図９参照）。
【００７９】
従って、本実施形態のＳ１５０では、回転量ｓφは次の式で演算することにより推定する。すなわち、回転角度φを推定する。
【００８０】
【数７】
【００８１】
【数８】
すなわち、回転量ｓφがエッジヒストグラムのシフト量si,(i=0,…,N-1)を上下２等分に分けることになる（図９参照）。
【００８２】
（アクティブセンサ１１の移動方向ωの推定）
次に、式（１）がsin曲線の周期性を保たれていることを利用して、アクティブセンサ１１の移動方向ωをロバストに推定する。
【００８３】
この推定の根拠は下記の通りである。
上述したように回転量ｓφを引いた後のエッジヒストグラムのシフト量ｓi’をsin曲線に近似する。しかし、図７に示しているように、これらのシフト量は、エッジの奥行きや空間の形に影響される。
【００８４】
エッジヒストグラムのシフト量ｓi’は、エッジの奥行きや空間の形の影響でsin曲線からずれるが、図７に示すように（０，π）の区間では、正の値を、（π，２π）の区間では、負の値をもっていると仮定することができる。
【００８５】
従って、シフト量ｓi’の符号を用いて、sin曲線をロバストに当てはめることができる。
ここで、回転角度φにより、シフト量ｓi’の−１，０，１の３つの値に変換し、−１と１の値に対応するシフト量ｓi’の中心がそれぞれなるべくsin曲線の負と正のピークに対応するように移動方向ωを決定する。
【００８６】
これは次の最大化問題になる。
【００８７】
【数９】
【００８８】
【数１０】
ここで、回転角度φに相当するｓφとｓφ±１を０にすることにより、エッジヒストグラムのシフト量ｓi’は符号へのノイズの影響を軽減することができる。
【００８９】
式（９）の左辺の微分を０とすると、次の方程式が得られる。
【００９０】
【数１１】
上の式（１１）から次のように、移動方向ωを直接求める。
【００９１】
【数１２】
なお、式（１１）は、式（９）の最大化のみならず、最小化も含んでいる。tan（θ）の周期がπであるから、移動方向ω又はω＋πのどちらかが式（９）を最大化することが分かる。
【００９２】
上記のようにして、本実施形態のＳ１５０では、式（１２）に基づいて、移動方向ωを算出する。Ｓ１５０では、上記のようにして、移動前地点に対するアクティブセンサ１１の回転角度φや移動方向ωを推定することができる。
【００９３】
１．６． Ｓ１６０（アクティブセンサ１１の移動距離の推定）
Ｓ１６０では、アクティブセンサ１１の移動距離の推定を行う。具体的には、図３（ｃ）のフローチャートに従って移動距離の推定を行う。
【００９４】
Ｓ１６２において、移動前にアクティブセンサ１１で得られた環境Ｋの全方位の距離データに基づくエッジの３次元点群を床平面（ｘ−ｙ平面）に写像し、図１０のようにエッジヒストグラム（床平面投影ヒストグラム）を生成する。この写像により、環境Ｋの壁などの垂直平面（床平面に対する垂直平面）にある多くの３次元点は同じ場所に投影され、垂直平面が存在する場所では、ヒストグラムの値は高くなる。なお、図１０において、ヒストグラムの値が大きな部分については、白く表している。
【００９５】
Ｓ１６４では、前記床平面投影ヒストグラムに対してハフ（Hough）変換を用いて、主な垂直平面（壁など）の方向を検出する。
（ハフ変換）
ハフ変換について説明する。図１４はハフ変換の説明図である。同図において、Ｘ−Ｙ平面上に、点Ｄ１から点Ｄ６が直線Ｒ上に分布しているものとする。原点Ｏから直線Ｒに下ろした垂線の足をＨ，ＯＨ＝ρ，ＯＨとＸ軸のなす角度をθ，Ｈの座標を（ｘ，ｙ）とすると、直線Ｒは下記の極座標の式で表現できる。
【００９６】
ρ＝ｘｃｏｓθ＋ｙｓｉｎθ
ここで、ρ，θを変数とした場合、ある点（ｘ，ｙ）を通るすべての直線群は、前記極座標の式で表現できる。
【００９７】
図１５はρ−θ平面上で、ある点を通る直線群を示した図である。ρ−θ平面上では、ある点を通る直線群は、唯一１本の曲線として表現でき、ρ，θが一意に決まれば、１本の直線が特定できることになる。
【００９８】
図１６はρ−θ平面上で複数の候補点の各々に対する直線群を示した図である。図１４に示した各候補点について、前記極座標の式に従って、図１５に示すように曲線を求めた一例を図１６に示している。そして、図１６に示すように、複数の曲線が描かれているが、すべての候補点に対し、最も適切な直線は、曲線同士が最も多く交差している点Ｑ（ρ0 ，θ0 ）から特定される直線ρ＝ｘｃｏｓθ0 ＋ｙｓｉｎθ0 である。
【００９９】
本実施形態においては、例えば図１０の床平面投影ヒストグラムに対してハフ変換を用いると、図１１に示すようにρ−θ平面上に、多数の曲線群が描かれる。なお、図１１では、曲線は白線で表されている。図１１において、曲線同士の公差が多い交点を、主な垂直平面（壁など）に関する直線として検出され、すなわち、垂直平面の方向が検出される。
【０１００】
本実施形態では、図１１のハフ変換のρ−θ平面が、ハフ投票空間とされている。このハフ投票空間から、θが所定角度（例えば６０度）以上離れた交点を検出することによって、図１０に示すように、（Ａ１，Ａ２）と（Ｂ１，Ｂ２）のような主な垂直平面の方向を検出する。
【０１０１】
そして、これらの主な垂直平面の方向のうち、移動方向ωに最も近い１つの垂直平面の方向をドミナント射影方向ｖとして選択し、図１０の床平面投影ヒストグラムを選択した垂直平面の方向(ドミナント射影方向ｖ)に沿って投影し、１次元ヒストグラムを生成する。
【０１０２】
なお、図１２は、Ａ１及びＡ２が互いに平行であって、その平行な方向である垂直平面の方向に沿って投影した方向をドミナント射影方向として、投影したときのヒストグラムを示し、Ａ１，Ａ２で示す部分は、ヒストグラムの値が大きいことを示している。図１３は、Ｂ１及びＢ２が互いに平行であって、その平行な方向である垂直平面の方向に沿って投影した方向をドミナント射影方向として、投影したときのヒストグラムを示し、Ｂ１，Ｂ２で示す部分は、ヒストグラムの値が大きいことを示している。
【０１０３】
Ｓ１６６では、移動後にアクティブセンサ１１にて得られた環境Ｋにおける全方位の距離データに基づくエッジの３次元点群をＺ軸（アクティブセンサ１１が走行した床平面に垂直な軸）を中心に、Ｓ１５０で推定した回転角度φに基づき、「−φ」で回転する。すなわち、
Ｐ’＝Ｒｚ（−φ）Ｐ
ここで、Ｒｚ（−φ）は回転マトリクスである。ＰとＰ’は回転前と回転後の３次元点を表している。
【０１０４】
Ｓ１６８では、Ｓ１６４で得られた主な垂直平面のヒストグラム投影方向をｖ１，ｖ２としたとき(図１０参照)、このヒストグラム投影方向ｖ１，ｖ２のうち、移動方向ωと最も近い方向(例えば、ｖ１)を選び、Ｐ’をその投影方向に投影し、ヒストグラムＨ’(ｖ１)を生成する。
【０１０５】
又、アクティブセンサ１１が走行移動前に、アクティブセンサ１１が得た環境Ｋの全方位の距離データに基づくエッジの３次元点をＰoとする。そして、Ｓ１６４で得られた主な垂直平面のヒストグラム投影方向をｖ１，ｖ２としたとき(図１０参照)、このヒストグラム投影方向ｖ１，ｖ２のうち、移動方向ωと最も近い方向(例えば、ｖ１)を選び、Ｐoをその投影方向に投影し、ヒストグラムＨ(ｖ１)を生成する。
【０１０６】
Ｓ１７０では、移動前と移動後で得られたｖ１方向（すなわち、移動方向ωと最も近い方向）のヒストグラムＨ(ｖ１)とＨ’(ｖ１)をマッチングし、２つのヒストグラムのシフト量λを求める。このシフト量λはアクティブセンサ１１が走行してヒストグラム投影方向ｖ１に沿った移動距離と関係する。
【０１０７】
Ｓ１８０では、前記シフト量λに基づいてアクティブセンサ１１が移動方向ωに沿って移動した移動距離Ｌを下記式(１３)を使用して演算する。
【０１０８】
【数１３】
２． Ｓ２０(予測距離画像の算出)
ここではアクティブセンサ１１が移動前に取得した全方位の距離データと、Ｓ１６０で取得した相対移動量(回転角度φ，移動方向ωと移動距離Ｌを含む)とに基づいて、移動後にアクティブセンサ１１の視点で得られる全方位距離画像を推定する。以下では、移動後の予測された全方位距離画像を単に予測距離画像という。
【０１０９】
本実施形態では、アクティブセンサ１１の各ステレオユニット１２により得られた距離画像を各ステレオユニット１２の配置パラメータを用いて、円筒座標系で表現し、統合することにより、全方位距離画像を生成する。
【０１１０】
図２６は、生成した全方位距離画像の例を示しており、横軸は方位角θ、縦軸は仰角γである。図１７は、仰角γの説明図であり、アクティブセンサ１１の視点中心から上を＋とし、下を−としている。なお、図２６は０＜θ＜２π、−π／３＜γ＜π／３の範囲を対象としている。
【０１１１】
ここで、移動前の全方位距離画像の方位角θ、仰角γ方向における距離値ｄs（θ，γ）は、移動後の相対移動量(回転角度φ，移動方向ωと移動距離Ｌを含む)により、方位角θ’、仰角γ’方向の位置に、距離値ｄp（θ’，γ’）に移動する。ここで、距離値ｄs（θ，γ）は、図１８に示すように、アクティブセンサ１１がポイントＳに位置したときの、ポイントＳから距離画像における任意の点Ｗまでの値である。距離値ｄp（θ’，γ’）は、アクティブセンサ１１がポイントＳからポイントＰに移動した後のポイントＰから距離画像における点Ｗまでの距離値である。
【０１１２】
方位角θ’、仰角γ’及び距離値ｄp（θ’，γ’）は、アクティブセンサ１１の回転角度φ、移動方向ωと、その移動距離Ｌを用いて、下記の式(１４)で求める。
【０１１３】
【数１４】
上記式(１４)により、アクティブセンサ１１が移動後の視点で得られる全方位距離画像、すなわち、予測距離画像が推定される。
【０１１４】
３． Ｓ３０（オクルージョン領域の推定）
Ｓ３０では、アクティブセンサ１１、すなわち、観測系が移動することによって、生じるオクルージョン領域を推定する。オクルージョンは、環境Ｋにおける動物体と、アクティブセンサ１１のカメラの移動量とに深く関係する。すなわち、奥行き(距離値)が急激に変化する部分にオクルージョンが生じる。
【０１１５】
このため、アクティブセンサ１１から得られる距離画像におけるジャンプエッジに注目し、距離画像と移動量に基づいて予測距離画像に生じるオクルージョン領域を推定する。
【０１１６】
ここで、移動前において取得した全方位距離画像におけるジャンプエッジの画素(ピクセル)を、(θ1，γ1，ｄs（θ1，γ1）)とする。そして、画素(θ1，γ1，ｄs（θ1，γ1）)の所定範囲内としての４−近傍にある距離値ｄs（θ1，γ1）より最も遠い距離値を有する画素を(θ2，γ2，ｄs（θ2，γ2）)とする。
【０１１７】
前記ジャンプエッジの画素は本発明のジャンプエッジ画素に相当し、所定範囲内としての４−近傍にあるｄs（θ1，γ1）より最も遠い画素は、対比対象画素に相当する。なお、本実施形態の「最も遠い距離値」は、本発明における「所定値」に相当する。 図１９は、オクルージョン領域の説明図である。同図において、ジャンプエッジの画素に対応する部分を(θ1，γ1，ｄs（θ1，γ1）)で示しており、その画素から最も遠い画素に相当する部位を(θ2，γ2，ｄs（θ2，γ2）)で示している。これらは、前記式(１４)より、アクティブセンサ１１の移動によって、それぞれ下記のように変換する。
【０１１８】
(θ1，γ1，ｄs（θ1，γ1）) → (θ1’，γ1’，ｄp（θ1’，γ1’）)
(θ2，γ2，ｄs（θ2，γ2）) → (θ2’，γ2’，ｄp（θ2’，γ2’）)
θ1’はジャンプエッジ画素の推定方位角，及びθ2’は対比対象画素の推定方位角に相当し、γ1’ジャンプエッジ画素の推定仰角，及びγ2’は対比対象画素の推定仰角に相当する。
【０１１９】
この変換により生じた各画素(ピクセル)の位置の差から方位角θと仰角γの方向のオクルージョン区間Ｉθ，Ｉγを推定する。又、オクルージョン区間Ｉθ，Ｉγから、次のようなオクルージョン領域Ｏｃ（θ，γ）を推定する。
【０１２０】
【数１５】
上記式(１５)中、Ｉθ＝（θ2’，θ1’）は、θ1＞θ2，θ1’＞θ2’のとき、移動後においては、方位角θ2’〜θ1’の間がオクルージョン区間を意味している。なお、θ1＞θ2，θ1’＞θ2’の場合は、動物体が、例えば、図１９においては、アクティブセンサ１１が右側に移動する場合に相当する。
【０１２１】
又、Ｉθ＝（θ1’，θ2’）は、θ2＞θ1，θ2’＞θ1’のとき、移動後においては、方位角θ1’〜θ2’間がオクルージョン区間を意味している。なお、θ2＞θ1，θ2’＞θ1’の場合は、例えば、図１９においては、アクティブセンサ１１が左側に移動する場合に相当する。
【０１２２】
そして、上記以外の場合は、方位角においてオクルージョン区間がないこと、すなわち空集合であることを意味する。
上記式中、Ｉγ＝（γ2’，γ1’）は、γ1＞γ2，γ1’＞γ2’のとき、予測距離画像においては、仰角γ2’〜γ1’間がオクルージョン区間を意味する。
【０１２３】
又、Ｉγ＝（γ1’，γ2’）は、γ2＞γ1，γ2’＞γ1’のとき、予測距離画像においては、仰角γ1’〜γ2’の間がオクルージョン区間を意味している。そして、上記以外の場合は、仰角においてオクルージョン区間がないこと、すなわち空集合であることを意味する。
【０１２４】
さらに、上記式(１５)では、予測距離画像における判定対象画素の仰角γが、推定仰角γ1’に一致し、予測距離画像における判定対象画素の方位角θがオクルージョン区間Ｉθ内にあるときは、Ｏ（θ，γ）＝１とする。すなわち、判定対象画素はオクルージョン領域内にあると判定する。
【０１２５】
又、予測距離画像における判定対象画素の方位角θが推定方位角θ1’に一致し、判定対象画素の仰角γがオクルージョン区間Ｉγ内にあるときは、Ｏ（θ，γ）＝１とする。すなわち、判定対象画素はオクルージョン領域内にあると判定する。
【０１２６】
そうでない場合には、Ｏ（θ，γ）＝０とする。
Ｏ（θ，γ）＝１の場合、予測距離画像における、方位角θ，仰角γの画素(ピクセル)はオクルージョン領域の一部であることを意味する。又、Ｏ（θ，γ）＝０の場合、方位角θ，仰角γの画素(ピクセル)はオクルージョン領域ではないことを意味している。
【０１２７】
４． Ｓ４０(差分による動物体領域の抽出)
Ｓ４０では、Ｓ２０で得られた予測距離画像と、アクティブセンサ１１が移動後に実際に得られる全方位距離画像との差分をとり、差分画像を生成し、Ｓ３０にて推定したオクルージョン領域を取り除くことにより、動物体領域を抽出する。具体的には、下記の通り行う。
【０１２８】
ここで、移動後取得する全方位距離画像、前記予測距離画像、及び生成する差分画像におけるθ，γに対する距離値をそれぞれｄg（θ，γ），ｄp（θ，γ），ｄδ（θ，γ）とする。又、差分画像の３値画像を得るための判定値をｄd（θ，γ）とする。なお、Ｓ２０では、予測距離画像の距離値は、ｄp（θ’，γ’）で示したが、この欄では、予測距離画像の方位角θ’と、仰角γ’は、移動後に取得した全方位距離画像の方位角θと、仰角γとそれぞれ等しいものとして、説明の便宜上、この欄では、ｄp（θ，γ）で示す。
【０１２９】
生成する差分画像の距離値ｄδ（θ，γ）は、差分画像の距離値＝(予測距離画像の距離値)−(移動後の全方位距離画像の距離値)、で得られる。すなわち、生成する差分画像の距離値ｄδ（θ，γ）は、
【０１３０】
【数１６】
で表される。又、この差分画像の距離値ｄδ（θ，γ）に対して閾値Ｔｈ、及び閾値−Ｔｈを用いて、差分画像の３値画像を得るための判定値を得る。なお、閾値Ｔｈ、及び閾値−Ｔｈは、差分画像の距離値ｄδ（θ，γ）に大きな変化があったかどうかを判定するための閾値である。
【０１３１】
【数１７】
式(１７)において、差分画像の距離値ｄδ（θ，γ）が閾値Ｔｈを超える場合であって、Ｏ（θ，γ）＝０、すなわち、画素(ピクセル)がオクルージョン領域にない場合は、その画素について判定値ｄd（θ，γ）を「１」とし、正の値とする。この画素(ピクセル)は、前記閾値Ｔｈを超えてアクティブセンサ１１に近い位置へ変化したことを示している。
【０１３２】
又、式(１７)において、差分画像の距離値ｄδ（θ，γ）が閾値−Ｔｈ未満の場合であって、Ｏ（θ，γ）＝０、すなわち、画素がオクルージョン領域にない場合は、判定値ｄd（θ，γ）を「−１」とし、負の値とする。この場合、画素(ピクセル)は、前記閾値−Ｔｈを超えてアクティブセンサ１１から離れた位置へ変化したことを示している。
【０１３３】
さらに、上記の条件を満足しない場合は、判定値ｄd（θ，γ）を「０」とする。すなわち、この場合、差分画像の距離値ｄδ（θ，γ）は、閾値−Ｔｈ〜閾値Ｔｈの変化であるため、画素(ピクセル)は、大きな変化がないことを示している。
【０１３４】
上記のようにした得られた判定値ｄd（θ，γ）に基づいて、前記差分画像を３値化する。
(具体例での説明)
１．具体例１
図２０で示す具体例１を参照して、式(１７)の判定値の設定について説明する。具体例１は、アクティブセンサ１１と、動物体２０とが、同一線上ではない、異なる方向にそれぞれ移動するときの例である。
【０１３５】
図２０は、アクティブセンサ１１の移動前後の位置と、動物体２０の移動前後の位置をそれぞれ示している。なお、説明の便宜上、以下は、アクティブセンサ１１において、仰角γ＝０における画素を前提として説明する。又、閾値Ｔｈを０とする。
【０１３６】
同図中、動物体２０及び、アクティブセンサ１１が移動後に実際に取得した全方位距離画像は、ａ６とａ７間の動物体２０(移動後)の円弧部分，ａ８〜ａ９〜ａ３〜ａ４〜ａ５の環境Ｋの壁を含んでいる。なお、各ａは、方位角θと関連し、以下では、説明の便宜上、例えば、ａ８を指す場合、方位角ａ８、又は、ａ８の方位角という。
【０１３７】
又、Ｓ２０で算出した予測距離画像は、ａ１〜ａ２の動物体２０(移動前)の円弧部分、ａ３〜ａ４〜ａ５〜ａ８〜ａ９の環境Ｋの壁を含んでいる。又、Ｓ３０で推定されたオクルージョン領域は、ａ３〜ａ４の環境Ｋの壁部分である。
【０１３８】
(ａ１〜ａ２の方位角)
さて、ａ１〜ａ２の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は動物体２０(移動前)の円弧部分のものであり、一方、移動後の全方位距離画像の距離値ｄg（θ，γ）は環境Ｋの壁部分のものであって、移動後の全方位距離画像の距離値ｄg（θ，γ）の方が、大きくなる。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＜０(＝−Ｔｈ)となる。又、この方位角ａ１〜ａ２は、Ｏ(θ，γ)＝０、すなわち、オクルージョン領域でない。この結果、方位角ａ１〜ａ２の範囲では、式(１７)の算出結果は、「−１」、すなわち負となる。なお、図２０では、式(１７)の算出結果を括弧書で示している。
【０１３９】
(ａ３〜ａ４の方位角)
ａ３〜ａ４の方位角の範囲は、オクルージョン領域であるため、Ｏ(θ，γ)＝１であり、この結果、方位角ａ３〜ａ４の範囲では、式(１７)の算出結果は、「０」となる。
【０１４０】
(ａ４〜ａ５の方位角)
ａ４〜ａ５の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は環境Ｋの壁部分のものである。又、一方、移動後の全方位距離画像の距離値ｄg（θ，γ）も同じ環境Ｋの壁部分のものである。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＝０となる。この結果、方位角ａ４〜ａ５の範囲では、式(１７)の算出結果は、「０」となる。
【０１４１】
(ａ６〜ａ７の方位角)
ａ６〜ａ７の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は環境Ｋの壁部分のものであり、一方、移動後の全方位距離画像の距離値ｄg（θ，γ）は動物体２０(移動後)の円弧部分のものである。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＞０(＝Ｔｈ)となる。又、この方位角ａ６〜ａ７は、Ｏ(θ，γ)＝０、すなわち、オクルージョン領域でない。この結果、方位角ａ６〜ａ７の範囲は、式(１７)の算出結果は、「１」、すなわち正となる。
【０１４２】
（ａ８〜ａ９の方位角）
ａ８〜ａ９の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は環境Ｋの壁部分のものである。又、一方、移動後の全方位距離画像の距離値ｄg（θ，γ）も同じ環境Ｋの壁部分のものである。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＝０となる。この結果、方位角ａ８〜ａ９の範囲では、式(１７)の算出結果は、「０」となる。
【０１４３】
上記のように、差分画像において、アクティブセンサ１１に近い位置へ変化した領域は、「正」の領域となり、一方、遠い位置へ変化した領域は「負」の領域で現れる。どちらの領域においても、移動後に得られる距離画像から、動物体２０を抽出したことになる。すなわち、「正」の領域は、動物体２０の移動後を抽出したことになり、「負」の領域は、動物体２０の移動前を抽出したことになる。
【０１４４】
なお、オクルージョン領域を考慮しない場合、動物体２０が移動後に現れるオクルージョン領域(先の例では、ａ３〜ａ４の方位角の領域)も動物体と検出してしまうことになる。しかし、Ｓ３０において、オクルージョン領域を推定し、推定したオクルージョン領域を式(１７)にて除外して区別しているため、動物体領域のみを検出できる。
【０１４５】
２． 具体例２
次に図２１及び図２２で示す具体例２を参照して、式(１７)の判定値の設定について説明する。具体例２は、アクティブセンサ１１と、動物体２０とが、同一線上において、互いに接近移動するときの例である。
【０１４６】
なお、この例では、Ｓ３０ではオクルージョン領域は、下記のように推定されている。図２２では、ジャンプエッジの画素に対応する部分を(θ1，γ1，ｄs（θ1，γ1）)で示しており、その画素から最も遠い画素に相当する部位を(θ2，γ2，ｄs（θ2，γ2）)で示す。
【０１４７】
この場合、式(１４)により、アクティブセンサ１１の移動により、
(θ1，γ1，ｄs（θ1，γ1）) → (θ1’，γ1’，ｄp（θ1’，γ1’）)
(θ2，γ2，ｄs（θ2，γ2）) → (θ2’，γ2’，ｄp（θ2’，γ2’）)
のように変換されている。なお、説明の便宜上、仰角γは０としている。
【０１４８】
そして、この場合、図２２に示すように、θ1＞θ2であり、かつ、θ２’＞θ１’であるため、式(１５)により、オクルージョン区間Ｉθは空集合となり、オクルージョン領域Ｏｃ（θ，γ）＝０、すなわち、オクルージョン領域はないとされている。なお、図２２では、ジャンプエッジは、動物体２０の右側の部分としたが、左側にもジャンプエッジが存在する。しかし、前述した同じ理由により、こちらの側もオクルージョン領域はないとされる。
【０１４９】
次に、Ｓ４０における処理を説明する。
図２１は、アクティブセンサ１１の移動前後の位置と、動物体２０の移動前後の位置をそれぞれ示している。なお、説明の便宜上、以下は、アクティブセンサ１１において、仰角γ＝０における画素を前提として説明する。又、閾値Ｔｈを０とする。同図中、動物体２０、及びアクティブセンサ１１が移動後に実際に取得した全方位距離画像は、ａ１〜ａ２〜ａ３〜ａ４間の動物体２０(移動後)の円弧部分，ａ４〜ａ１間の環境Ｋの壁を含んでいる。
【０１５０】
又、Ｓ２０で算出した予測距離画像は、ａ１〜ａ２間の環境Ｋの壁、ａ２〜ａ３間の動物体２０(移動前)の円弧部分、ａ３〜ａ４〜ａ１の環境Ｋの壁を含んでいる。
【０１５１】
(ａ１〜ａ２の方位角)
さて、ａ１〜ａ２の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は環境Ｋの壁部分のものであり、一方、移動後の全方位距離画像の距離値ｄg（θ，γ）は動物体２０(移動後)の円弧部分のものである。従って、予測距離画像の距離値ｄp（θ，γ）の方が、移動後の全方位距離画像の距離値ｄg（θ，γ）のよりも大きくなる。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＞０(＝Ｔｈ)となる。又、この方位角ａ１〜ａ２は、Ｏ(θ，γ)＝０、すなわち、オクルージョン領域でない。この結果、方位角ａ１〜ａ２の範囲では、式(１７)の算出結果は、「１」、すなわち正となる。なお、図２１では、式(１７)の算出結果を括弧書で示している。
【０１５２】
(ａ２〜ａ３の方位角)
ａ２〜ａ３の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は移動前の動物体２０の円弧部分のものである。一方、移動後の全方位距離画像の距離値ｄg（θ，γ）はアクティブセンサ１１に接近移動後の動物体２０の円弧部分のものである。このため、距離値ｄp（θ，γ）＞距離値ｄg（θ，γ）となり、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＞０(＝Ｔｈ)となる。又、この方位角ａ２〜ａ３は、Ｏ(θ，γ)＝０、すなわち、オクルージョン領域でない。この結果、方位角ａ２〜ａ３の範囲では、式(１７)の算出結果は、「１」、すなわち正となる。
【０１５３】
(ａ３〜ａ４の方位角)
ａ３〜ａ４の方位角の範囲では、ａ１〜ａ２と同じ理由により、式(１７)の算出結果は、「１」、すなわち正となる。
【０１５４】
(ａ４〜ａ１の方位角)
ａ４〜ａ１の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）及び移動後の全方位距離画像の距離値ｄg（θ，γ）は同じ環境Ｋの壁部分のものである。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＝０となる。この結果、方位角ａ４〜ａ１の範囲では、式(１７)の算出結果は、「０」となる。
【０１５５】
このように、差分画像において、アクティブセンサ１１に近い位置へ変化した領域は、「正」の領域となり、移動後に得られる距離画像から、動物体２０を抽出したことになる。すなわち、「正」の領域は、動物体２０の移動後を抽出したことになる。
【０１５６】
３． 具体例３
次に図２３で示す具体例３を参照して、式(１７)の判定値の設定について説明する。具体例３は、アクティブセンサ１１と、動物体２０とが、同一線上において、同方向に移動するときの例である。この場合、具体例２と同じ理由で、Ｓ３０では、オクルージョン領域はないとされる。
【０１５７】
Ｓ４０における処理を説明する。
図２３は、アクティブセンサ１１の移動前後の位置と、動物体２０の移動前後の位置をそれぞれ示している。なお、説明の便宜上、以下は、アクティブセンサ１１において、仰角γ＝０における画素を前提として説明する。又、閾値Ｔｈを０とする。同図中、動物体２０、及びアクティブセンサ１１が移動後に実際に取得した全方位距離画像は、ａ１〜ａ２間の環境Ｋの壁、ａ２〜ａ３間の動物体２０(移動後)の円弧部分，ａ３〜ａ４〜ａ１の環境Ｋの壁を含んでいる。
【０１５８】
又、Ｓ２０で算出した予測距離画像は、ａ１〜ａ２間の動物体２０(移動前)の円弧部分、ａ２〜ａ３間の動物体２０(移動前)の円弧部分、ａ３〜ａ４間の動物体２０(移動前)の円弧部分、ａ４〜ａ１の環境Ｋの壁を含んでいる。
【０１５９】
(ａ１〜ａ２の方位角)
ａ１〜ａ２の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は動物体２０(移動前)の円弧部分のものであり、一方、移動後の全方位距離画像の距離値ｄg（θ，γ）は環境Ｋの壁のものである。従って、予測距離画像の距離値ｄp（θ，γ）の方が、移動後の全方位距離画像の距離値ｄg（θ，γ）のよりも小さくなる。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＜０(＝−Ｔｈ)となる。又、この方位角ａ１〜ａ２は、Ｏ(θ，γ)＝０、すなわち、オクルージョン領域でない。この結果、方位角ａ１〜ａ２の範囲では、式(１７)の算出結果は、「−１」、すなわち負となる。なお、図２３では、式(１７)の算出結果を括弧書で示している。
【０１６０】
(ａ２〜ａ３の方位角)
ａ２〜ａ３の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は移動前の動物体２０の円弧部分のものである。一方、移動後の全方位距離画像の距離値ｄg（θ，γ）はアクティブセンサ１１から離間移動後の動物体２０の円弧部分のものである。このため、距離値ｄp（θ，γ）＜距離値ｄg（θ，γ）となり、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＜０(＝−Ｔｈ)となる。又、この方位角ａ２〜ａ３は、Ｏ(θ，γ)＝０、すなわち、オクルージョン領域でない。
【０１６１】
この結果、方位角ａ２〜ａ３の範囲では、式(１７)の算出結果は、「−１」、すなわち負となる。
(ａ３〜ａ４の方位角)
ａ３〜ａ４の方位角の範囲では、ａ１〜ａ２と同じ理由により、式(１７)の算出結果は、「−１」、すなわち負となる。
【０１６２】
(ａ４〜ａ１の方位角)
ａ４〜ａ１の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）及び移動後の全方位距離画像の距離値ｄg（θ，γ）は同じ環境Ｋの壁部分のものである。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＝０となる。この結果、方位角ａ４〜ａ１の範囲では、式(１７)の算出結果は、「０」となる。
【０１６３】
このように、差分画像において、アクティブセンサ１１から遠い位置へ変化した領域は、「負」の領域となり、移動後に得られる距離画像から、動物体２０を抽出したことになる。すなわち、「負」の領域は、動物体２０の移動前を抽出したことになる。
【０１６４】
アクティブセンサ１１と動物体２０とが同一直線上において、移動する場合、具体例２及び具体例３以外に、互いに離間する方向に移動する場合や、具体例３とは１８０度反対向きに、アクティブセンサ１１と動物体２０が移動する場合もある。これらの場合も、同様に、動物体２０の領域が「正」又は「負」となり、抽出される。
【０１６５】
さて具体例の説明を終了して、フローチャートの説明に戻る。
Ｓ４０では、前述のように処理した後、３値画像に対して、ノイズ除去処理を行い、Ｓ４０の処理を終了する。なお、ノイズ除去処理は、例えば、画素数（ピクセル数）が所定閾値以下のものを、ノイズとして除去する処理である。
【０１６６】
本実施形態によれば、下記に示す効果を有する。
（１） 本実施形態のアクティブセンサ１１の動物体検出装置では、コンピュータ１６(相対移動量推定手段)は、アクティブセンサ１１の移動前と移動後の、移動方向、移動距離、回転量を含む相対移動量を推定するようにした。又。コンピュータ１６(予測距離画像生成手段)は、移動前に取得した全方位距離画像と、前記推定した相対移動量に基づいて、移動後の予測距離画像を推定して生成するようにした。さらに、コンピュータ１６(オクルージョン領域推定手段)は、アクティブセンサ１１が移動することにより、前記予測距離画像に生ずるオクルージョン領域を推定するようにした。そして、コンピュータ１６(動物体領域抽出手段)は、前記予測距離画像と、移動後にアクティブセンサ１１が取得した全方位距離画像との差分画像を生成する際に、前記オクルージョン領域を取り除き、動物体領域を抽出するようにした。
【０１６７】
又、本実施形態のアクティブセンサの動物体検出プログラムは、コンピュータ１６を上記各手段として機能させるようにした。
又、アクティブセンサ１１の動物体検出方法は、第１ステップとして、アクティブセンサ１１の移動前後の、移動方向、移動距離、回転量を含む相対移動量を推定するようにした。又、第２ステップとして、移動前に取得した全方位距離画像と、前記第１ステップにて推定した相対移動量に基づいて、移動後の予測距離画像を推定して生成するようにした。さらに、第３ステップとして、アクティブセンサ１１が移動することにより、前記予測距離画像に生ずるオクルージョン領域を推定するようにした。又、第４ステップとして、前記予測距離画像と、移動後にアクティブセンサ１１が取得した全方位距離画像との差分画像を生成する際に、前記オクルージョン領域を取り除き、動物体領域を抽出するようにした。
【０１６８】
この結果、観測系が移動することにより生ずる動物体以外の環境の「見かけの変化」と、人物等の動物体による環境変化を容易に区別して動物体の検出を容易にでき、オクルージョン領域を推定して除去することにより、より正確に動物体による環境変化のみを検出することができる効果を奏する。
【０１６９】
（２） 本実施形態では、コンピュータ１６は、移動前の全方位距離画像のジャンプエッジ画素と、同画素の４−近傍（所定範囲内）に位置する画素の中で、アクティブセンサ１１から最も遠い距離値を有する対比対象画素について、相対移動量に基づき、それぞれ移動後の推定方位角、及び推定仰角を求めた。そして、コンピュータ１６は、ジャンプエッジ画素と対比対象画素のそれぞれの推定方位角の区間、及び推定仰角の範囲を、前記予測距離画像における前記オクルージョン領域のオクルージョン区間であると推定するようにした。
【０１７０】
又、本実施形態のアクティブセンサの動物体検出プログラムは、コンピュータ１６を上記手段として機能させるようにした。
又、本実施形態では、第３ステップとして、移動前の全方位距離画像のジャンプエッジ画素と、同画素の４−近傍（所定範囲内）に位置する画素の中で、アクティブセンサ１１から最も遠い距離値を有する対比対象画素について、相対移動量に基づき、それぞれ移動後の推定方位角、及び推定仰角を求めた。そして、第３ステップでは、ジャンプエッジ画素と対比対象画素のそれぞれの推定方位角の区間、及び推定仰角の範囲を、前記予測距離画像における前記オクルージョン領域のオクルージョン区間であると推定するようにした。
【０１７１】
この結果、オクルージョン領域を推定するためのオクルージョン区間を、容易に決定できる。
（３） 本実施形態では、コンピュータ１６は、オクルージョン領域推定手段として、判定対象画素の仰角が、ジャンプエッジ画素の推定仰角に一致し、かつ、判定対象画素の方位角が推定方位角で定めたオクルージョン区間内にあるときは、判定対象画素を、オクルージョン領域内にあると判定するようにした。
【０１７２】
又、本実施形態のアクティブセンサの動物体検出プログラムは、コンピュータ１６を上記手段として機能させるようにした。
又、本実施形態では、第３ステップとして、判定対象画素の仰角が、ジャンプエッジ画素の推定仰角に一致し、かつ、判定対象画素の方位角が推定方位角で定めたオクルージョン区間内にあるとき(条件１)は、判定対象画素は、オクルージョン領域内にあると判定するようにした。
【０１７３】
この結果、条件１が成立したときに、オクルージョン領域の推定を容易にできる。
（４） 又、コンピュータ１６は、オクルージョン領域推定手段として、判定対象画素の方位角が、ジャンプエッジ画素の推定方位角に一致し、かつ、判定対象画素の仰角が推定仰角で定めたオクルージョン区間内にあるとき(条件２)は、判定対象画素をオクルージョン領域内にあると判定するようにした。
【０１７４】
又、本実施形態のアクティブセンサの動物体検出プログラムは、コンピュータ１６を上記手段として機能させるようにした。
又、第３ステップとして、判定対象画素の方位角が、ジャンプエッジ画素の推定方位角に一致し、かつ、判定対象画素の仰角が推定仰角で定めたオクルージョン区間内にあるとき(条件２)は、判定対象画素をオクルージョン領域内にあると判定するようにした。
【０１７５】
この結果、条件２が成立したときに、オクルージョン領域の推定を容易にできる。
（５） 本実施形態では、コンピュータ１６は、動物体領域抽出手段として、差分画像を生成する際に、各画素の距離値の差に基づいて、正及び負の符号判定を行い、この正又は負の符号判定に基づいて、動物体領域抽出を行う。
【０１７６】
又、本実施形態のアクティブセンサの動物体検出プログラムは、コンピュータ１６を上記手段として機能させるようにした。
又、第４ステップとして、差分画像を生成する際に、各画素の距離値の差に基づいて、正及び負の符号判定を行い、この正及び負の符号判定に基づいて、動物体領域抽出を行う。
【０１７７】
この結果、正及び負の符号判定を行うことにより、移動前と移動後の両方の動物体領域抽出を容易に行うことができる。
(実験例)
図２４〜図３１は、実験室において、アクティブセンサ１１は、人物が位置する方向に向かって移動するとともに、動物体としての人物が実験室の壁側に向かって移動する前後の画像を取得し画像処理した結果を示している。なお、各図において、横軸は、方位角θ（０＜θ＜３６０度）であり、縦軸は、仰角γ(−３／３＜γ＜π／３)の範囲を対象として図示している。
【０１７８】
図２４は「移動前」、図２５は「移動後」のもので、両図は、距離画像ではなく、全方位のカラー画像で取得したものを説明の便宜上、白黒の濃淡画像で示している。図２６は図２４に対応した全方位距離画像、図２７は、図２５に対応した距離画像である。
【０１７９】
そして、得られた相対移動量を用いて、図２６から生成した予測距離画像を図２８に、推定したオクルージョン領域を、図２９に示している。図３０は、図２８の予測距離画像と移動後取得した全方位距離画像(図２７)の差分をとり、距離値が「正」であった領域を白、「負」であった領域をグレーで示している。ここでは、グレーで示された領域は、アクティブセンサ１１が移動前存在していたものが、移動後なくなっていることを示している。又、白で示す領域は、アクティブセンサ１１が移動前なかったものが、移動後現れた領域を示している。図２８において、人物像の右側輪郭に沿って現れている黒い領域は、オクルージョン領域推定に相当する。この領域は、図２９で示す、推定したオクルージョン領域にて取り除き、移動後現れた領域のみを図３１に示す。
【０１８０】
このように、オクルージョン領域を考慮することにより、動物体を精度良く検出できている。
なお、本発明の実施形態は上記実施形態に限定されるものではなく、発明の趣旨を逸脱しない範囲で、適宜に変更して次のように実施することもできる。
【０１８１】
（１） 前記実施形態では、アクティブセンサ１１として、「”実環境センシングのための全方位ステレオシステム（ＳＯＳ）”、電気学会論文誌C.Vol.121-C,No.5,pp.876-881.2001」に記載されているものを使用した。これに限らず、他の全方位カメラから得られた全方位距離画像を入力するようにしてもよい。
【０１８２】
(２) 前記実施形態では、コンピュータ１６は、動物体領域抽出手段として、差分画像を生成する際に、各画素の距離値の差に基づいて、正及び負の符号判定を行い、この正及び負の符号判定に基づいて、動物体領域抽出を行った。
【０１８３】
これに代えて、コンピュータ１６は、動物体領域抽出手段として、差分画像を生成する際に、各画素の距離値の差に基づいて、正又は負のいずれかの符号判定を行い、この正又は負のいずれかの符号判定に基づいて、動物体領域抽出を行うようにしてもよい。
【０１８４】
この場合、移動前、或いは、移動後の動物体領域抽出を行うことができる。
(３) 前記実施形態では、Ｓ３０において、移動前において取得した全方位距離画像におけるジャンプエッジの画素(ピクセル)を、(θ1，γ1，ｄs（θ1，γ1）)とする。そして、画素(θ1，γ1，ｄs（θ1，γ1）)の所定範囲内としての４−近傍にある距離値ｄs（θ1，γ1）より最も遠い距離値を有する画素を(θ2，γ2，ｄs（θ2，γ2）)とした。そして、「所定値」とは、最も遠い距離値を意味するようにしたが、これに限定するものではない。
【０１８５】
例えば、動物体の移動速度と、アクティブセンサの移動速度が予め分かっており、両者の移動範囲が分かっている場合には、ジャンプエッジと４−近傍にある画素との距離値がとりうる範囲を予め予測できる。この場合には、予測できる距離値の範囲の中で、所定値として予め定数を決定しておき、距離値がこの定数以上の複数の画素を対比対象画素としてもよい。
【０１８６】
(４) 前記実施形態では、ジャンプエッジ画素に対する所定範囲内として４−近傍としたが、８−近傍や１６−近傍等であってもよい。 (５) 前記実施形態では、式(１５)において、予測距離画像における判定対象画素の仰角γが、推定仰角γ1’に一致し、予測距離画像における判定対象画素の方位角θがオクルージョン区間Ｉθ内にあるときは、Ｏ（θ，γ）＝１とする。すなわち、判定対象画素はオクルージョン領域内にあると判定した。又、予測距離画像における判定対象画素の方位角θが推定方位角θ1’に一致し、判定対象画素の仰角γがオクルージョン区間Ｉγ内にあるときは、Ｏ（θ，γ）＝１とする。すなわち、判定対象画素はオクルージョン領域内にあると判定した。そうでない場合には、Ｏ（θ，γ）＝０とした。
【０１８７】
これに代えて、上記(３)の場合のように、対比対象画素が複数になった場合、前記複数の対比対象画素で囲まれる範囲をオクルージョン領域として判定してもよい。
【０１８８】
【発明の効果】
以上詳述したように請求項１乃至請求項１０に記載の発明によれば、観測系が移動することにより生ずる動物体以外の環境の「見かけの変化」と、人物等の動物体による環境変化を容易に区別して動物体の検出を容易にできる。そして、オクルージョン領域を推定して除去することにより、より正確に動物体による環境変化のみを検出することができる効果を奏する。
【０１８９】
請求項１１乃至請求項１５の発明によれば、アクティブセンサの動物体検出方法及び動物体検出装置を容易に実現できる動物体検出プログラムを提供できる。
【図面の簡単な説明】
【図１】 アクティブセンサ１１の電気的構成のブロック図。
【図２】 アクティブセンサ１１の機械的構成の概略図。
【図３】 （ａ）〜（ｃ）はコンピュータ１６が実行する位置・姿勢推定プログラムのフローチャート。
【図４】 図５に対応するパノラマエッジ画像の説明図。
【図５】 エッジヒストグラムの例を示す説明図。
【図６】 アクティブセンサ１１の平行移動におけるエッジヒストグラムのシフトを説明するための説明図。
【図７】 （ａ）はアクティブセンサ１１を中心にした円筒座標系の説明図、（ｂ）は、アクティブセンサ１１を中心にした四角筒座標系の説明図、（ｃ）は、異なる空間系にエッジを写像したときの、ヒストグラムのシフトとsin曲線を表した説明図。
【図８】 アクティブセンサ１１の２つの地点における全方位エッジヒストグラムのマッチングコストマトリクスＣ（s,i）の説明図。
【図９】 総合シフト量から回転量ｓφと移動方向ωを求める方法の説明図。
【図１０】 床平面投影ヒストグラムの説明図。
【図１１】 本実施形態において、ρ−θ表面上で複数の候補点の各々に退位する直線群を示した図。
【図１２】 ドミナント射影方向のヒストグラム。
【図１３】 図１２とは異なるドミナント射影方向のヒストグラム。
【図１４】ハフ変換の説明図。
【図１５】ρ−θ表面上で、ある点を通る直線群を示した図。
【図１６】ρ−θ表面上で複数の候補点の各々に退位する直線群を示した図。
【図１７】仰角γの説明図。
【図１８】距離値の説明図。
【図１９】オクルージョン領域の説明図。
【図２０】具体例１の式(１７)の判定値の設定の説明図。
【図２１】具体例２の式(１７)の判定値の設定の説明図。
【図２２】同じく具体例２のジャンプエッジの画素に関する説明図。
【図２３】具体例３の式(１７)の判定値の設定の説明図。
【図２４】 アクティブセンサ１１の「移動前」における白黒の濃淡画像。
【図２５】 アクティブセンサ１１の「移動後」における白黒の濃淡画像。
【図２６】図２４に対応した全方位距離画像。
【図２７】図２５に対応した距離画像。
【図２８】予測距離画像。
【図２９】推定して生成されたオクルージョン領域を示す画像。
【図３０】差分画像。
【図３１】動物体移動後の動体検出された画像。
【符号の説明】
１１…アクティブセンサ
１２…ステレオユニット
１６…コンピュータ（相対移動量推定手段、予測距離画像生成手段、オクルージョン領域推定手段、及び動物体領域抽出手段）
【発明の属する技術分野】
本発明は、アクティブセンサの動物体検出装置及び動物体検出方法、並びに動物体検出プログラムに関するものである。
【０００２】
【従来の技術】
近年、人間と共存する移動ロボットの開発が盛んに行われている。ロボットが自律移動するためには、外界情報を獲得し環境を把握する必要がある。環境を認識する上で有用なセンサとして視覚センサが挙げられる。これまで多くの移動体ビジョンの研究は、静止環境下で移動するためのものである。しかし、人間と共存するロボットを実現するためには、人物等の動物体が存在する動的環境下でも衝突を回避し、安全に移動することや、人物を追跡するため、移動体自身が移動しつつ動物体を検出することが求められている（非特許文献１〜３）。
【０００３】
観測系が移動する場合、取得される画像には、観測系の移動（視点移動）による「見かけの変化」と、人物等の「動物体による環境自身の変化」が含まれる。動物体を検出するとは、「環境自身の変化」のみを抽出することであり、この２つの変化を区別する必要がある。観測系の移動による「見かけの変化」には、静止環境の相対運動による変化と、オクルージョンによる変化がある。視点移動に伴う静止環境の相対運動による変化は、画像においては静止環境の位置や大きさの変化として表れる。オクルージョンによる変化とは、移動前には見えていない領域が視点が移動することにより見えたり、見えていた領域が見えなくなったりする等の見かけの変化である。
【０００４】
このオクルージョンは、特にカメラに対して物体が近い場合、領域として大きく現れ、人物等の動物体による環境変化と区別することが困難になる。ロボットが自律移動を行うためには、ロボットの近くの情報、変化の検出が重要となる。
【０００５】
従来、観測系の運動を考慮し、動物体を検出する方法としてオプティカルフローを用いて推定する方法が提案されている（非特許文献４〜７）。これらは、静止物体の領域のオプティカルフローが、消失点を中心として放射状に分布する性質を利用して、その性質を満足しない領域を動物体として検出している。
【０００６】
【非特許文献１】
石川繁樹，浅香俊一，“移動障害物を含むような動的変化を伴う走行環境における自律移動ロボットの走行誘導方式”，日本ロボット学会誌学術論文，Vol.11,No.6,pp.856-867,1993.
【非特許文献２】
井上晃，井上健司，大川善邦，“複数移動障害物の行動予測に基づく自律移動ロボットのオンライン回避行動”，日本ロボット学会誌学術論文，Vol.15,No.2,pp.249-260,1997.
【非特許文献３】
松村幸輝，村井保之，“遺伝的プログラミングに基づくファジーロボットの障害物回避”，電子情報通信学会誌学術論文誌 A，Vol.J83-A,No.12,pp.1539-1551,2000.
【非特許文献４】
太田直哉，“信頼性情報をもったオプティカルフローからの形状復元とその移動物体検出への応用”，電子情報通信学会誌学術論文誌(D-II), Vol.J76-D-II,No.8,pp.1562-1571,1993.
【非特許文献５】
武田信之，渡辺睦，小野口一則，“消失点推定残差法を用いた移動障害物検出”，第１回ロボティクスシンポジア予稿集，pp.29-34,1996.
【非特許文献６】
海老根巧，浜田望，“観測系の運動を考慮したオプティカルフロー推定に基づく運動体検出”，電子情報通信学会誌学術論文誌(D-II), Vol.J83-D-II,No.6,pp.1498-1506,2000.
【非特許文献７】
岡田隆三，白井良明，三浦純，久野義徳“オプティカルフローと距離情報に基づく動物体追跡”，電子情報通信学会誌学術論文誌(D-II), Vol.J80-D-II,No.6,pp.1530-1538,1997.
【０００７】
【発明が解決しようとする課題】
しかし、これらのオプティカルフローを用いた方法では、原理的に光軸に平行な運動の時がフローベクトルが小さいため、背景と動物体とを区別することが難しいという問題がある。又、急激な見かけの変化が生じることのないよう、時間移動量が小さいと仮定しているが、例えば、カメラと動物体までの距離が近い場合では、少しの移動でも見かけの変化が大きくなり、動物体を正確に抽出することが困難である。
【０００８】
本発明は、観測系の移動による動物体以外の環境の見かけの変化と、動物体の移動による環境変化とを容易に区別して動物体の検出が容易にできるアクティブセンサの動物体検出方法及び動物体検出装置を提供することを目的としている。又、オクルージョン領域を推定することにより、より正確に動物体を検出することができるアクティブセンサの動物体検出方法及び動物体検出装置を提供することを目的としている。
【０００９】
又、上記アクティブセンサの動物体検出方法及び動物体検出装置を容易に実現できる動物体検出プログラムを提供することを目的としている。
【００１０】
【課題を解決するための手段】
上記問題点を解決するため、請求項１記載の発明は、全方位距離画像を取得可能であり、環境内を移動可能なアクティブセンサの動物体検出装置において、前記アクティブセンサの移動前と移動後の、移動方向、移動距離、回転量を含む相対移動量を推定する相対移動量推定手段と、移動前に取得した全方位距離画像と、前記相対移動量推定手段にて推定した相対移動量に基づいて、移動後の全方位距離画像(以下、予測距離画像という)を推定して生成する予測距離画像生成手段と、前記アクティブセンサが移動することにより、前記予測距離画像に生ずるオクルージョン領域を推定するオクルージョン領域推定手段と、前記予測距離画像と、移動後にアクティブセンサが取得した全方位距離画像との差分画像を生成する際に、前記オクルージョン領域を取り除き、動物体領域を抽出する動物体領域抽出手段とを備えたことを特徴とするアクティブセンサの動物体検出装置を要旨とするものである。
【００１１】
請求項２の発明は、請求項１において、前記オクルージョン領域推定手段は、移動前に取得した全方位距離画像のジャンプエッジの画素(以下、ジャンプエッジ画素という)と、同画素の所定範囲内に位置する画素の中で、アクティブセンサから所定値以上の距離値を有する画素(以下、対比対象画素という)について、前記相対移動量に基づき、それぞれ移動後の推定方位角、及び推定仰角を求め、前記ジャンプエッジ画素と対比対象画素のそれぞれの推定方位角の区間、及び推定仰角の範囲を、前記予測距離画像における前記オクルージョン領域のオクルージョン区間であると推定することを特徴とする。
【００１２】
請求項３の発明は、請求項２において、前記オクルージョン領域推定手段は、前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の仰角が、前記ジャンプエッジ画素の推定仰角に一致し、かつ、前記判定対象画素の方位角が前記推定方位角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定することを特徴とする。
【００１３】
請求項４の発明は、請求項２において、前記オクルージョン領域推定手段は、前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の方位角が、前記ジャンプエッジ画素の推定方位角に一致し、かつ、前記判定対象画素の仰角が前記推定仰角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定することを特徴とする。
【００１４】
請求項５の発明は、請求項１乃至請求項４のうちいずれか１項において、前記動物体領域抽出手段は、前記差分画像を生成する際に、各画素の距離値の差に基づいて、正、負のうち、少なくともいずれか一方の符号判定を行い、この符号判定に基づいて、動物体領域抽出を行うことを特徴とする。
【００１５】
請求項６の発明は、全方位距離画像を取得可能であり、環境内を移動可能なアクティブセンサの動物体検出方法において、前記アクティブセンサの移動前と移動後の、移動方向、移動距離、回転量を含む相対移動量を推定する第１ステップと、移動前に取得した全方位距離画像と、前記第１ステップにて推定した相対移動量に基づいて、移動後の全方位距離画像(以下、予測距離画像という)を推定して生成する第２ステップと、前記アクティブセンサが移動することにより、前記予測距離画像に生ずるオクルージョン領域を推定する第３ステップと、前記予測距離画像と、移動後にアクティブセンサが取得した全方位距離画像との差分画像を生成する際に、前記オクルージョン領域を取り除き、動物体領域を抽出する第４ステップを含むことを特徴とするアクティブセンサの動物体検出方法を要旨とするものである。
【００１６】
請求項７の発明は、請求項６において、前記第３ステップは、移動前に取得した全方位距離画像のジャンプエッジの画素(以下、ジャンプエッジ画素という)と、同画素の所定範囲内に位置する画素の中で、アクティブセンサから所定値以上の距離値を有する画素(以下、対比対象画素という)について、前記相対移動量に基づき、それぞれ移動後の推定方位角、及び推定仰角を求め、前記ジャンプエッジ画素と対比対象画素のそれぞれの推定方位角の区間、及び推定仰角の範囲を、前記予測距離画像における前記オクルージョン領域のオクルージョン区間であると推定することを含むことを特徴とする。
【００１７】
請求項８の発明は、請求項７において、前記第３ステップは、前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の仰角が、前記ジャンプエッジ画素の推定仰角に一致し、かつ、前記判定対象画素の方位角が前記推定方位角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定することを特徴とする。
【００１８】
請求項９の発明は、請求項７において、前記第３ステップは、前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の方位角が、前記ジャンプエッジ画素の推定方位角に一致し、かつ、前記判定対象画素の仰角が前記推定仰角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定することを特徴とする。
【００１９】
請求項１０の発明は、請求項６乃至請求項９のうちいずれか１項において、前記第４ステップは、前記差分画像を生成する際に、各画素の距離値の差に基づいて、正、負のうち、少なくともいずれか一方の符号判定を行い、この符号判定に基づいて、動物体領域抽出を行うことを特徴とする。
【００２０】
請求項１１の発明は、コンピュータを、アクティブセンサの移動前と移動後の、移動方向、移動距離、回転量を含む相対移動量を推定する相対移動量推定手段と、移動前に取得した全方位距離画像と、前記相対移動量推定手段にて推定した相対移動量に基づいて、移動後の全方位距離画像(以下、予測距離画像という)を推定して生成する予測距離画像生成手段と、前記アクティブセンサが移動することにより、前記予測距離画像に生ずるオクルージョン領域を推定するオクルージョン領域推定手段と、前記予測距離画像と、移動後にアクティブセンサが取得した全方位距離画像との差分画像を生成する際に、前記オクルージョン領域を取り除き、動物体領域を抽出する動物体領域抽出手段として、機能させることを特徴とするアクティブセンサの動物体検出プログラムを要旨とするものである。
【００２１】
請求項１２の発明は、請求項１１において、さらに、コンピュータを、前記オクルージョン領域推定手段として、移動前に取得した全方位距離画像のジャンプエッジの画素(以下、ジャンプエッジ画素という)と、同画素の所定範囲内に位置する画素の中で、アクティブセンサから所定値以上の距離値を有する画素(以下、対比対象画素という)について、前記相対移動量に基づき、それぞれ移動後の推定方位角、及び推定仰角を求め、前記ジャンプエッジ画素と対比対象画素のそれぞれの推定方位角の区間、及び推定仰角の範囲を、前記予測距離画像における前記オクルージョン領域のオクルージョン区間であると推定するように機能させることを特徴とする。
【００２２】
請求項１３の発明は、請求項１２において、さらに、コンピュータを、前記オクルージョン領域推定手段として、前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の仰角が、前記ジャンプエッジ画素の推定仰角に一致し、かつ、前記判定対象画素の方位角が前記推定方位角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定するように機能させることを特徴とする。
【００２３】
請求項１４の発明は、請求項１２において、さらに、コンピュータを、前記オクルージョン領域推定手段として、前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の方位角が、前記ジャンプエッジ画素の推定方位角に一致し、かつ、前記判定対象画素の仰角が前記推定仰角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定するように機能させることを特徴とする。
【００２４】
請求項１５の発明は、請求項１１乃至請求項１４のうちいずれか１項において、
さらに、コンピュータを前記動物体領域抽出手段として、前記差分画像を生成する際に、各画素の距離値の差に基づいて、正、負のうち、少なくともいずれか一方の符号判定を行い、この符号判定に基づいて、動物体領域抽出を行うことを特徴とする。
【００２５】
【発明の実施の形態】
以下、本発明のアクティブセンサの動物体検出装置を具体化した一実施形態を図１〜図２３を参照して説明する。
【００２６】
本実施形態のアクティブセンサの動物体検出装置は、アクティブセンサ１１とコンピュータ１６にて構成されている。
図１はアクティブセンサの電気的構成を示すブロック図である。図２はアクティブセンサ１１の機械的構成の概略図である。
【００２７】
アクティブセンサ１１は、複数の車輪を有する車体βを備えており、車体β内に設けられた図示しない電動モータにより前記車輪が駆動されて環境Ｋ内の任意の位置に向けて自動走行（直線及び曲線に沿った走行を含む）及びが可能である。なお、図２においては、説明の便宜上、環境Ｋは、車体βに比較して、小さく図示している。
【００２８】
アクティブセンサ１１は、複数の３眼ステレオユニット（以下、単にステレオユニット１２という）、メモリユニット１５、同期信号発生器１７等を備えている。メモリユニット１５、コンピュータ１６、同期信号発生器１７等は車体β内に格納されている。
【００２９】
前記複数のステレオユニット１２は、撮像手段に相当する。又、前記コンピュータ１６は、相対移動量推定手段、予測距離画像生成手段、オクルージョン領域推定手段、及び動物体領域抽出手段に相当する。
【００３０】
アクティブセンサ１１は、３次元空間内の、全ての方向(以下、全方位という)におけるカラー画像と３次元情報(距離画像)を、同時刻にリアルタイムで取得することが可能な装置である。ステレオユニット１２は、３個のビデオカメラからなり、各ステレオユニット１２は正２０面体の各面上にそれぞれ配置されている。そして、各ステレオユニット１２は同特性を備え、各面に配置したステレオユニット１２により、全方位のカラー画像と全方位の白黒画像（以下、距離データという）を同一時刻にリアルタイムで取得可能なシステムである。これによって、３次元空間上の全ての方向におけるカラー画像と３次元情報を同一時刻に得ることができる。
【００３１】
又、同じ特性を持つステレオユニット１２を正２０面体の各面上に配置したことにより、３次元空間を均等に分割し、高解像度の情報の取得が可能である。なお、このアクティブセンサ１１は、「”実環境センシングのための全方位ステレオシステム（ＳＯＳ）”、電気学会論文誌C.Vol.121-C,No.5,pp.876-881,2001」に記載されている。
【００３２】
ステレオユニット１２では、図１に示すように、１つの基準ビデオカメラＶＣｓと、一対の参照用ビデオカメラＶＣとからなる。そして、参照用ビデオカメラＶＣは基準ビデオカメラＶＣｓの光軸を交線として、互いに直交する一対の平面に含ませるように配置されている。そして、これらのカメラにより、２つのステレオペアを構成するように配置されている。
【００３３】
各ステレオユニット１２からは、１枚のカラー画像と２枚の白黒画像とからなるステレオ画像が取得され、全方位の２０枚のカラー画像と４０枚の白黒画像を１セットとして１５セット／秒でメモリユニット１５に転送する。メモリユニット１５は、転送されてきた全方位の画像データを記憶する。
【００３４】
各ステレオユニット１２の各ビデオカメラには、同期信号発生器１７から共通の外部同期信号が供給されている。このことにより、ディジタル化されたフレームにおいて、完全に同期した画像データが得られる。
【００３５】
コンピュータ１６は、所定の周期毎に、同コンピュータ１６内に備えているＲＯＭ１６ａに予め格納したアクティブセンサの動物体検出プログラムを実行する。前記ＲＯＭ１６ａを備えたコンピュータ１６は記憶手段に相当する。
【００３６】
又、コンピュータ１６は、メモリユニット１５にアクセスし、全方位のカラー画像と距離データをその時々に取得する。
（作用）
さて、以上のように構成されたアクティブセンサの動物体検出装置の作用を図３〜図２３を参照して説明する。
【００３７】
図３（ａ）は、コンピュータ１６が実行するアクティブセンサの動物体検出プログラムのフローチャートであり、コンピュータ１６は、所定周期毎に実行し、アクティブセンサ１１の動物体検出を行う。
【００３８】
以下、各ステップ毎に説明する。
１．Ｓ１０（自己位置・姿勢推定）
Ｓ１０では、アクティブセンサ１１の自己位置・姿勢推定を行う。具体的には図３（ｂ）のＳ１１０〜Ｓ１６０の処理を行う。
【００３９】
１．１． Ｓ１１０（エッジの検出）
Ｓ１１０では、エッジ検出を行う。
すなわち、基準ビデオカメラＶＣｓ（センタカメラ）が取得したカラー画像を濃淡化してＬｏＧ（Laplasian of Gaussian）フィルタを通し、ゼロクロス点をエッジとして検出する。
【００４０】
１．２． Ｓ１２０（最大エッジ勾配の算出）
次に、最大エッジ勾配の算出を行う。
すなわち、Sobelフィルタにて各エッジ画素におけるエッジの勾配（強度）を計算し、画像中の最大エッジ勾配（すなわち、最大エッジ強度）を求める。
【００４１】
これは、ＬｏＧフィルタはノイズに影響されやすいため、ノイズや照明の影響で偽のエッジ多く存在するためであり、これを除くための処理である。
この処理によって、強度が最大エッジ勾配値の５％未満のエッジ画素をノイズとしてエッジから削除する。
【００４２】
１．３． Ｓ１３０（現在位置のエッジヒストグラムの生成）
次に、Ｓ１３０において、各ステレオユニット１２のセンタカメラである基準ビデオカメラＶＣｓで得られたエッジ点から、３６０°のパノラマエッジ画像座標系に写像し、縦方向投影のエッジヒストグラムを生成する。エッジヒストグラムは統計量に相当する。
【００４３】
前記３６０°のパノラマエッジ画像は３６０°の円筒画像ともいう。すなわち、この円筒画像におけるエッジを縦方向（円筒の軸心に沿った方向）に投影して、全方位エッジヒストグラムを生成するのである。
【００４４】
図５は、生成された全方位エッジヒストグラムの例を示している。図５において、横軸は０〜３６０°の範囲を示し、縦軸はそのヒストグラムである。図４は、図５に対応するパノラマエッジ画像を示している。
【００４５】
（位置・姿勢と全方位エッジヒストグラムとの関係）
ここで、アクティブセンサ１１の位置・姿勢とヒストグラムとの関係を説明する。
【００４６】
アクティブセンサ１１の移動や回転は、アクティブセンサ１１で得られた全方位の画像のエッジヒストグラムのシフトを引き起こす。
アクティブセンサ１１の回転によって生じたエッジヒストグラムの回転量はすべての方位角において一定である。しかし、アクティブセンサ１１の平行移動で生じたエッジヒストグラムの移動量は移動方向とエッジの方位角に関係する。
【００４７】
図６に示すように、アクティブセンサ１１が移動前地点αからある移動方向ωに向いて動いたとき、ω±ｎπ，ｎ＝０，１の方位角においてはエッジヒストグラムの移動量が小さい。ｎ＝０は移動方向ωであり、ｎ＝１は移動方向ωとは反対方向の意味である。
【００４８】
なお、総合シフト量とは、前記回転量と移動量の合計の量である。又、移動方向ωは、図６に示すように移動前地点αを中心として所定の方向の向きを０°としたとき、その０°からω°離れた方向をいう。
【００４９】
ω±（２ｎ＋１）π／２の方位角においてはそのエッジヒストグラムの移動量が大きくなる。
アクティブセンサ１１が移動方向ωに沿う移動距離をＬとし、移動しながら回転角度φにて回転したとする。
【００５０】
この場合、ある方位角θに位置するエッジピクセルの円筒座標系（３６０°のパノラマエッジ画像座標系）における総合シフト量δθは次の式(１)で決まる。
【００５１】
【数１】
【００５２】
式（１）は、エッジの奥行きに影響されるが、図７（ｃ）に示すように、sinで近似することができ、sin曲線と同様に２πの周期を持つ。その総合シフト量δθの符号はπ周期毎に反転する。
【００５３】
なお、図７（ｃ）は全方位エッジヒストグラムの総合シフト量δθを式（１）で計算した場合の曲線とsin曲線を表した説明図である。
同図において、Ａはsin曲線（正弦波曲線）であり、Ｂは、円筒形の部屋でアクティブセンサ１１が動いた場合の、全方位エッジヒストグラムの総合シフト量δθを式（１）に基づいて演算した曲線である（図７（ａ）参照）。
【００５４】
又、Ｃは正方形の部屋でアクティブセンサ１１が動いた場合の、全方位エッジヒストグラムの総合シフト量δθを式（１）に基づいて演算した曲線である（図７（ｂ）参照）。
【００５５】
本実施形態では、式（１）の符号がπ周期毎に反転する特徴を利用して、全方位エッジヒストグラムの総合シフト量δθからアクティブセンサ１１の移動方向ωと回転角度φを簡単かつロバストに推定するのである。
【００５６】
１．４． Ｓ１４０（動的計画法によるマッチング：ＤＰマッチング）
Ｓ１４０では、アクティブセンサ１１の現在位置(移動後地点)の全方位エッジヒストグラムと移動前地点の全方位エッジヒストグラムをＤＰマッチングし、それぞれの方位角（各移動前地点の方位角）におけるヒストグラムの総合シフト量δθを求める。
【００５７】
移動後地点の全方位エッジヒストグラムは現在位置ヒストグラムに相当し、移動前地点の全方位エッジヒストグラムは移動前地点ヒストグラムに相当する。
この総合シフト量δθを求めることにより、移動前地点に対するアクティブセンサ１１の移動後地点の移動方向ωと回転角度φを算出する。
【００５８】
以下、移動前地点に対する、アクティブセンサ１１の移動後地点の全方位エッジヒストグラムの総合シフト量δθの算出の仕方を詳細に説明する。
（動的計画法マッチング（ＤＰマッチング）について）
ここでは、移動後地点の全方位エッジヒストグラムと、ある移動前地点の全方位エッジヒストグラムのＤＰマッチングについて、すなわち、２つの全方位エッジヒストグラムをＤＰマッチングする方法を説明する。
【００５９】
移動前地点と移動後地点の全方位エッジヒストグラムのそれぞれを
移動前地点：Ｈｐ＝[hp(i),i=0,…,…,N-1]
と
移動後地点：Ｈｃ＝[hc(j),j=0,…,…,N-1｝
とする。
【００６０】
Ｎは３６０の倍数であり、エッジヒストグラムを生成するときの方位角θで決まる。本実施形態では、Ｎ＝７２０である。すなわち、エッジヒストグラムの角度分解能は０．５°である。移動前地点の全方位エッジヒストグラムＨｐの１つのピンhp(i)は、移動と回転によってアクティブセンサ１１の移動後地点の全方位エッジヒストグラムＨｃにおいてシフト量siが生じたとき、hp(i)とhc(i+si)と似ていると仮定することができる。
【００６１】
hp(i)とhc(i+si)との差の二乗をhp(i)とhc(i+si)との間のマッチングコストとすると、次のマッチングコストマトリクスＣ（s,i）が得られる。
【００６２】
【数２】
hc(i+si)≡hc(i+si−N)とする。
【００６３】
[(hp(i),hc(i+si)),i=0,…,N-1]が正しいマッチングである場合、それらのマッチングペアはマッチングコストマトリクスＣ（s,i）の中でコストが低く、かつ式（１）の形をした曲線をなす。ここでは、(hp(i),hc(i+si))はhp(i)とhc(i+si)とのマッチングペアを表している。
【００６４】
図８は、アクティブセンサ１１の２つの地点における全方位エッジヒストグラムのマッチングコストマトリクスＣ（s,i）を示す。なお、説明の便宜上、マトリクス中のマッチングコストの低いパスをセンタリングし、上下をカットしている。又、図中、「.-+*&%#$@ABCD」の各記号は、コストのレベルをそれぞれ示し、「.-+*&%#$@ABCD」の順番は、左から右に向かってコストの低い順から高い順に並べている。すなわち、マッチングコストは「.」＜「-」＜「+」＜「*」＜「&」＜「%」＜「#」＜「$」＜「@」＜「A」＜「B」＜「C」＜「D」の大小関係となっている。
【００６５】
図中、縦軸はエッジのシフト量si、横軸はiであり、方位角θに相当する。又、図中、Ｃ（s,i）の中のsin曲線に似たコストの低い曲線は各方位角θにおけるエッジヒストグラムの総合シフト量δθに対応する。Ｃ（s,i）の中のsin曲線に似たコストの低い曲線を探索することで、ＨｐとＨｃの間の全方位ヒストグラムの総合シフト量δθが求められる。
【００６６】
本実施形態では、計算コストの低い動的計画法（ＤＰ）を用いて、Ｃ（s,i）から周期２πを持ち、かつ連続した最小コストのパスを求め、そのパスからロバストに回転角度φと移動方向ωを推定する。
【００６７】
Ｃ（s,i）から周期２πを持ちかつ連続した最小コストのパスは次の条件付き最小化問題に定義することができる。
【００６８】
【数３】
【００６９】
hp(i)とhc(j)の周期がNであるため、Ｃ（s,i）のインデックスのsとiに関しては、s±Ｎ→sとi±Ｎ→iで計算される。
パスが特定の行ｋから始まる（すなわち、ｓ０＝ｋ）と仮定した場合には、式（２）の最小化は次のように動的計画法で求めることができる。
【００７０】
【数４】
【数５】
【数６】
【００７３】
ｋ＝０，…，Ｎ−１に対して、上記のようにＣmin(k)を計算し、そのうち、
最小値Ｃ^min(k)＝Ｃmin(k^)
を求める。
【００７４】
Ｃmin(k^)が対応しているパスがＨｐとＨｃの間の最適マッチングとする。
以下、上記パスを最適マッチングパスという。
上記計算により、２πの周期を持ち、かつ連続性のあるエッジヒストグラムのマッチングパスを探索できる。
【００７５】
そして、以下、移動後地点の全方位エッジヒストグラムと他の移動前地点の全方位エッジヒストグラムのマッチングについても同様に処理する。
１．５． Ｓ１５０（姿勢と移動方向の推定）
Ｓ１５０では、Ｓ１４０で得られたアクティブセンサ１１の移動後地点と移動前地点の全方位エッジヒストグラムの総合シフト量から移動前地点に対するアクティブセンサ１１の移動方向ωと回転角度φを推定する。
【００７６】
（回転角度φの推定）
まず、アクティブセンサ１１の回転角度φの推定について説明する。
ＤＰマッチングで得られた最小コストパスであるsi,(i=0,…,N-1)は、アクティブセンサ１１の移動後地点と移動前地点のそれぞれの全方位エッジヒストグラムＨｃとＨｐの間の総合シフト量を表している。図９中のsin曲線の近傍に示された波形はＤＰマッチングで得られた最小コストパスを示す。
【００７７】
式（１）で示したように、これらの総合シフト量はアクティブセンサ１１の回転角度φで生じたヒストグラム全体の回転量と、移動方向ωにおける平行移動で生じた各方位角での移動量からなる。
【００７８】
式（１）から分かるように、ヒストグラムの総合シフト量は回転角度φに相当する回転量ｓφを引けば、引いた後のシフト量はπ周期毎に反転する。すなわち、回転角度φを中心にπ周期で、上下反転する（図９参照）。
【００７９】
従って、本実施形態のＳ１５０では、回転量ｓφは次の式で演算することにより推定する。すなわち、回転角度φを推定する。
【００８０】
【数７】
【数８】
【００８２】
（アクティブセンサ１１の移動方向ωの推定）
次に、式（１）がsin曲線の周期性を保たれていることを利用して、アクティブセンサ１１の移動方向ωをロバストに推定する。
【００８３】
この推定の根拠は下記の通りである。
上述したように回転量ｓφを引いた後のエッジヒストグラムのシフト量ｓi’をsin曲線に近似する。しかし、図７に示しているように、これらのシフト量は、エッジの奥行きや空間の形に影響される。
【００８４】
エッジヒストグラムのシフト量ｓi’は、エッジの奥行きや空間の形の影響でsin曲線からずれるが、図７に示すように（０，π）の区間では、正の値を、（π，２π）の区間では、負の値をもっていると仮定することができる。
【００８５】
従って、シフト量ｓi’の符号を用いて、sin曲線をロバストに当てはめることができる。
ここで、回転角度φにより、シフト量ｓi’の−１，０，１の３つの値に変換し、−１と１の値に対応するシフト量ｓi’の中心がそれぞれなるべくsin曲線の負と正のピークに対応するように移動方向ωを決定する。
【００８６】
これは次の最大化問題になる。
【００８７】
【数９】
【数１０】
【００８９】
式（９）の左辺の微分を０とすると、次の方程式が得られる。
【００９０】
【数１１】
【００９１】
【数１２】
【００９２】
上記のようにして、本実施形態のＳ１５０では、式（１２）に基づいて、移動方向ωを算出する。Ｓ１５０では、上記のようにして、移動前地点に対するアクティブセンサ１１の回転角度φや移動方向ωを推定することができる。
【００９３】
１．６． Ｓ１６０（アクティブセンサ１１の移動距離の推定）
Ｓ１６０では、アクティブセンサ１１の移動距離の推定を行う。具体的には、図３（ｃ）のフローチャートに従って移動距離の推定を行う。
【００９４】
Ｓ１６２において、移動前にアクティブセンサ１１で得られた環境Ｋの全方位の距離データに基づくエッジの３次元点群を床平面（ｘ−ｙ平面）に写像し、図１０のようにエッジヒストグラム（床平面投影ヒストグラム）を生成する。この写像により、環境Ｋの壁などの垂直平面（床平面に対する垂直平面）にある多くの３次元点は同じ場所に投影され、垂直平面が存在する場所では、ヒストグラムの値は高くなる。なお、図１０において、ヒストグラムの値が大きな部分については、白く表している。
【００９５】
Ｓ１６４では、前記床平面投影ヒストグラムに対してハフ（Hough）変換を用いて、主な垂直平面（壁など）の方向を検出する。
（ハフ変換）
ハフ変換について説明する。図１４はハフ変換の説明図である。同図において、Ｘ−Ｙ平面上に、点Ｄ１から点Ｄ６が直線Ｒ上に分布しているものとする。原点Ｏから直線Ｒに下ろした垂線の足をＨ，ＯＨ＝ρ，ＯＨとＸ軸のなす角度をθ，Ｈの座標を（ｘ，ｙ）とすると、直線Ｒは下記の極座標の式で表現できる。
【００９６】
ρ＝ｘｃｏｓθ＋ｙｓｉｎθ
ここで、ρ，θを変数とした場合、ある点（ｘ，ｙ）を通るすべての直線群は、前記極座標の式で表現できる。
【００９７】
図１５はρ−θ平面上で、ある点を通る直線群を示した図である。ρ−θ平面上では、ある点を通る直線群は、唯一１本の曲線として表現でき、ρ，θが一意に決まれば、１本の直線が特定できることになる。
【００９８】
図１６はρ−θ平面上で複数の候補点の各々に対する直線群を示した図である。図１４に示した各候補点について、前記極座標の式に従って、図１５に示すように曲線を求めた一例を図１６に示している。そして、図１６に示すように、複数の曲線が描かれているが、すべての候補点に対し、最も適切な直線は、曲線同士が最も多く交差している点Ｑ（ρ0 ，θ0 ）から特定される直線ρ＝ｘｃｏｓθ0 ＋ｙｓｉｎθ0 である。
【００９９】
本実施形態においては、例えば図１０の床平面投影ヒストグラムに対してハフ変換を用いると、図１１に示すようにρ−θ平面上に、多数の曲線群が描かれる。なお、図１１では、曲線は白線で表されている。図１１において、曲線同士の公差が多い交点を、主な垂直平面（壁など）に関する直線として検出され、すなわち、垂直平面の方向が検出される。
【０１００】
本実施形態では、図１１のハフ変換のρ−θ平面が、ハフ投票空間とされている。このハフ投票空間から、θが所定角度（例えば６０度）以上離れた交点を検出することによって、図１０に示すように、（Ａ１，Ａ２）と（Ｂ１，Ｂ２）のような主な垂直平面の方向を検出する。
【０１０１】
そして、これらの主な垂直平面の方向のうち、移動方向ωに最も近い１つの垂直平面の方向をドミナント射影方向ｖとして選択し、図１０の床平面投影ヒストグラムを選択した垂直平面の方向(ドミナント射影方向ｖ)に沿って投影し、１次元ヒストグラムを生成する。
【０１０２】
なお、図１２は、Ａ１及びＡ２が互いに平行であって、その平行な方向である垂直平面の方向に沿って投影した方向をドミナント射影方向として、投影したときのヒストグラムを示し、Ａ１，Ａ２で示す部分は、ヒストグラムの値が大きいことを示している。図１３は、Ｂ１及びＢ２が互いに平行であって、その平行な方向である垂直平面の方向に沿って投影した方向をドミナント射影方向として、投影したときのヒストグラムを示し、Ｂ１，Ｂ２で示す部分は、ヒストグラムの値が大きいことを示している。
【０１０３】
Ｓ１６６では、移動後にアクティブセンサ１１にて得られた環境Ｋにおける全方位の距離データに基づくエッジの３次元点群をＺ軸（アクティブセンサ１１が走行した床平面に垂直な軸）を中心に、Ｓ１５０で推定した回転角度φに基づき、「−φ」で回転する。すなわち、
Ｐ’＝Ｒｚ（−φ）Ｐ
ここで、Ｒｚ（−φ）は回転マトリクスである。ＰとＰ’は回転前と回転後の３次元点を表している。
【０１０４】
Ｓ１６８では、Ｓ１６４で得られた主な垂直平面のヒストグラム投影方向をｖ１，ｖ２としたとき(図１０参照)、このヒストグラム投影方向ｖ１，ｖ２のうち、移動方向ωと最も近い方向(例えば、ｖ１)を選び、Ｐ’をその投影方向に投影し、ヒストグラムＨ’(ｖ１)を生成する。
【０１０５】
又、アクティブセンサ１１が走行移動前に、アクティブセンサ１１が得た環境Ｋの全方位の距離データに基づくエッジの３次元点をＰoとする。そして、Ｓ１６４で得られた主な垂直平面のヒストグラム投影方向をｖ１，ｖ２としたとき(図１０参照)、このヒストグラム投影方向ｖ１，ｖ２のうち、移動方向ωと最も近い方向(例えば、ｖ１)を選び、Ｐoをその投影方向に投影し、ヒストグラムＨ(ｖ１)を生成する。
【０１０６】
Ｓ１７０では、移動前と移動後で得られたｖ１方向（すなわち、移動方向ωと最も近い方向）のヒストグラムＨ(ｖ１)とＨ’(ｖ１)をマッチングし、２つのヒストグラムのシフト量λを求める。このシフト量λはアクティブセンサ１１が走行してヒストグラム投影方向ｖ１に沿った移動距離と関係する。
【０１０７】
Ｓ１８０では、前記シフト量λに基づいてアクティブセンサ１１が移動方向ωに沿って移動した移動距離Ｌを下記式(１３)を使用して演算する。
【０１０８】
【数１３】
ここではアクティブセンサ１１が移動前に取得した全方位の距離データと、Ｓ１６０で取得した相対移動量(回転角度φ，移動方向ωと移動距離Ｌを含む)とに基づいて、移動後にアクティブセンサ１１の視点で得られる全方位距離画像を推定する。以下では、移動後の予測された全方位距離画像を単に予測距離画像という。
【０１０９】
本実施形態では、アクティブセンサ１１の各ステレオユニット１２により得られた距離画像を各ステレオユニット１２の配置パラメータを用いて、円筒座標系で表現し、統合することにより、全方位距離画像を生成する。
【０１１０】
図２６は、生成した全方位距離画像の例を示しており、横軸は方位角θ、縦軸は仰角γである。図１７は、仰角γの説明図であり、アクティブセンサ１１の視点中心から上を＋とし、下を−としている。なお、図２６は０＜θ＜２π、−π／３＜γ＜π／３の範囲を対象としている。
【０１１１】
ここで、移動前の全方位距離画像の方位角θ、仰角γ方向における距離値ｄs（θ，γ）は、移動後の相対移動量(回転角度φ，移動方向ωと移動距離Ｌを含む)により、方位角θ’、仰角γ’方向の位置に、距離値ｄp（θ’，γ’）に移動する。ここで、距離値ｄs（θ，γ）は、図１８に示すように、アクティブセンサ１１がポイントＳに位置したときの、ポイントＳから距離画像における任意の点Ｗまでの値である。距離値ｄp（θ’，γ’）は、アクティブセンサ１１がポイントＳからポイントＰに移動した後のポイントＰから距離画像における点Ｗまでの距離値である。
【０１１２】
方位角θ’、仰角γ’及び距離値ｄp（θ’，γ’）は、アクティブセンサ１１の回転角度φ、移動方向ωと、その移動距離Ｌを用いて、下記の式(１４)で求める。
【０１１３】
【数１４】
【０１１４】
３． Ｓ３０（オクルージョン領域の推定）
Ｓ３０では、アクティブセンサ１１、すなわち、観測系が移動することによって、生じるオクルージョン領域を推定する。オクルージョンは、環境Ｋにおける動物体と、アクティブセンサ１１のカメラの移動量とに深く関係する。すなわち、奥行き(距離値)が急激に変化する部分にオクルージョンが生じる。
【０１１５】
このため、アクティブセンサ１１から得られる距離画像におけるジャンプエッジに注目し、距離画像と移動量に基づいて予測距離画像に生じるオクルージョン領域を推定する。
【０１１６】
ここで、移動前において取得した全方位距離画像におけるジャンプエッジの画素(ピクセル)を、(θ1，γ1，ｄs（θ1，γ1）)とする。そして、画素(θ1，γ1，ｄs（θ1，γ1）)の所定範囲内としての４−近傍にある距離値ｄs（θ1，γ1）より最も遠い距離値を有する画素を(θ2，γ2，ｄs（θ2，γ2）)とする。
【０１１７】
前記ジャンプエッジの画素は本発明のジャンプエッジ画素に相当し、所定範囲内としての４−近傍にあるｄs（θ1，γ1）より最も遠い画素は、対比対象画素に相当する。なお、本実施形態の「最も遠い距離値」は、本発明における「所定値」に相当する。 図１９は、オクルージョン領域の説明図である。同図において、ジャンプエッジの画素に対応する部分を(θ1，γ1，ｄs（θ1，γ1）)で示しており、その画素から最も遠い画素に相当する部位を(θ2，γ2，ｄs（θ2，γ2）)で示している。これらは、前記式(１４)より、アクティブセンサ１１の移動によって、それぞれ下記のように変換する。
【０１１８】
(θ1，γ1，ｄs（θ1，γ1）) → (θ1’，γ1’，ｄp（θ1’，γ1’）)
(θ2，γ2，ｄs（θ2，γ2）) → (θ2’，γ2’，ｄp（θ2’，γ2’）)
θ1’はジャンプエッジ画素の推定方位角，及びθ2’は対比対象画素の推定方位角に相当し、γ1’ジャンプエッジ画素の推定仰角，及びγ2’は対比対象画素の推定仰角に相当する。
【０１１９】
この変換により生じた各画素(ピクセル)の位置の差から方位角θと仰角γの方向のオクルージョン区間Ｉθ，Ｉγを推定する。又、オクルージョン区間Ｉθ，Ｉγから、次のようなオクルージョン領域Ｏｃ（θ，γ）を推定する。
【０１２０】
【数１５】
【０１２１】
又、Ｉθ＝（θ1’，θ2’）は、θ2＞θ1，θ2’＞θ1’のとき、移動後においては、方位角θ1’〜θ2’間がオクルージョン区間を意味している。なお、θ2＞θ1，θ2’＞θ1’の場合は、例えば、図１９においては、アクティブセンサ１１が左側に移動する場合に相当する。
【０１２２】
そして、上記以外の場合は、方位角においてオクルージョン区間がないこと、すなわち空集合であることを意味する。
上記式中、Ｉγ＝（γ2’，γ1’）は、γ1＞γ2，γ1’＞γ2’のとき、予測距離画像においては、仰角γ2’〜γ1’間がオクルージョン区間を意味する。
【０１２３】
又、Ｉγ＝（γ1’，γ2’）は、γ2＞γ1，γ2’＞γ1’のとき、予測距離画像においては、仰角γ1’〜γ2’の間がオクルージョン区間を意味している。そして、上記以外の場合は、仰角においてオクルージョン区間がないこと、すなわち空集合であることを意味する。
【０１２４】
さらに、上記式(１５)では、予測距離画像における判定対象画素の仰角γが、推定仰角γ1’に一致し、予測距離画像における判定対象画素の方位角θがオクルージョン区間Ｉθ内にあるときは、Ｏ（θ，γ）＝１とする。すなわち、判定対象画素はオクルージョン領域内にあると判定する。
【０１２５】
又、予測距離画像における判定対象画素の方位角θが推定方位角θ1’に一致し、判定対象画素の仰角γがオクルージョン区間Ｉγ内にあるときは、Ｏ（θ，γ）＝１とする。すなわち、判定対象画素はオクルージョン領域内にあると判定する。
【０１２６】
そうでない場合には、Ｏ（θ，γ）＝０とする。
Ｏ（θ，γ）＝１の場合、予測距離画像における、方位角θ，仰角γの画素(ピクセル)はオクルージョン領域の一部であることを意味する。又、Ｏ（θ，γ）＝０の場合、方位角θ，仰角γの画素(ピクセル)はオクルージョン領域ではないことを意味している。
【０１２７】
４． Ｓ４０(差分による動物体領域の抽出)
Ｓ４０では、Ｓ２０で得られた予測距離画像と、アクティブセンサ１１が移動後に実際に得られる全方位距離画像との差分をとり、差分画像を生成し、Ｓ３０にて推定したオクルージョン領域を取り除くことにより、動物体領域を抽出する。具体的には、下記の通り行う。
【０１２８】
ここで、移動後取得する全方位距離画像、前記予測距離画像、及び生成する差分画像におけるθ，γに対する距離値をそれぞれｄg（θ，γ），ｄp（θ，γ），ｄδ（θ，γ）とする。又、差分画像の３値画像を得るための判定値をｄd（θ，γ）とする。なお、Ｓ２０では、予測距離画像の距離値は、ｄp（θ’，γ’）で示したが、この欄では、予測距離画像の方位角θ’と、仰角γ’は、移動後に取得した全方位距離画像の方位角θと、仰角γとそれぞれ等しいものとして、説明の便宜上、この欄では、ｄp（θ，γ）で示す。
【０１２９】
生成する差分画像の距離値ｄδ（θ，γ）は、差分画像の距離値＝(予測距離画像の距離値)−(移動後の全方位距離画像の距離値)、で得られる。すなわち、生成する差分画像の距離値ｄδ（θ，γ）は、
【０１３０】
【数１６】
【０１３１】
【数１７】
【０１３２】
又、式(１７)において、差分画像の距離値ｄδ（θ，γ）が閾値−Ｔｈ未満の場合であって、Ｏ（θ，γ）＝０、すなわち、画素がオクルージョン領域にない場合は、判定値ｄd（θ，γ）を「−１」とし、負の値とする。この場合、画素(ピクセル)は、前記閾値−Ｔｈを超えてアクティブセンサ１１から離れた位置へ変化したことを示している。
【０１３３】
さらに、上記の条件を満足しない場合は、判定値ｄd（θ，γ）を「０」とする。すなわち、この場合、差分画像の距離値ｄδ（θ，γ）は、閾値−Ｔｈ〜閾値Ｔｈの変化であるため、画素(ピクセル)は、大きな変化がないことを示している。
【０１３４】
上記のようにした得られた判定値ｄd（θ，γ）に基づいて、前記差分画像を３値化する。
(具体例での説明)
１．具体例１
図２０で示す具体例１を参照して、式(１７)の判定値の設定について説明する。具体例１は、アクティブセンサ１１と、動物体２０とが、同一線上ではない、異なる方向にそれぞれ移動するときの例である。
【０１３５】
図２０は、アクティブセンサ１１の移動前後の位置と、動物体２０の移動前後の位置をそれぞれ示している。なお、説明の便宜上、以下は、アクティブセンサ１１において、仰角γ＝０における画素を前提として説明する。又、閾値Ｔｈを０とする。
【０１３６】
同図中、動物体２０及び、アクティブセンサ１１が移動後に実際に取得した全方位距離画像は、ａ６とａ７間の動物体２０(移動後)の円弧部分，ａ８〜ａ９〜ａ３〜ａ４〜ａ５の環境Ｋの壁を含んでいる。なお、各ａは、方位角θと関連し、以下では、説明の便宜上、例えば、ａ８を指す場合、方位角ａ８、又は、ａ８の方位角という。
【０１３７】
又、Ｓ２０で算出した予測距離画像は、ａ１〜ａ２の動物体２０(移動前)の円弧部分、ａ３〜ａ４〜ａ５〜ａ８〜ａ９の環境Ｋの壁を含んでいる。又、Ｓ３０で推定されたオクルージョン領域は、ａ３〜ａ４の環境Ｋの壁部分である。
【０１３８】
(ａ１〜ａ２の方位角)
さて、ａ１〜ａ２の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は動物体２０(移動前)の円弧部分のものであり、一方、移動後の全方位距離画像の距離値ｄg（θ，γ）は環境Ｋの壁部分のものであって、移動後の全方位距離画像の距離値ｄg（θ，γ）の方が、大きくなる。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＜０(＝−Ｔｈ)となる。又、この方位角ａ１〜ａ２は、Ｏ(θ，γ)＝０、すなわち、オクルージョン領域でない。この結果、方位角ａ１〜ａ２の範囲では、式(１７)の算出結果は、「−１」、すなわち負となる。なお、図２０では、式(１７)の算出結果を括弧書で示している。
【０１３９】
(ａ３〜ａ４の方位角)
ａ３〜ａ４の方位角の範囲は、オクルージョン領域であるため、Ｏ(θ，γ)＝１であり、この結果、方位角ａ３〜ａ４の範囲では、式(１７)の算出結果は、「０」となる。
【０１４０】
(ａ４〜ａ５の方位角)
ａ４〜ａ５の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は環境Ｋの壁部分のものである。又、一方、移動後の全方位距離画像の距離値ｄg（θ，γ）も同じ環境Ｋの壁部分のものである。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＝０となる。この結果、方位角ａ４〜ａ５の範囲では、式(１７)の算出結果は、「０」となる。
【０１４１】
(ａ６〜ａ７の方位角)
ａ６〜ａ７の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は環境Ｋの壁部分のものであり、一方、移動後の全方位距離画像の距離値ｄg（θ，γ）は動物体２０(移動後)の円弧部分のものである。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＞０(＝Ｔｈ)となる。又、この方位角ａ６〜ａ７は、Ｏ(θ，γ)＝０、すなわち、オクルージョン領域でない。この結果、方位角ａ６〜ａ７の範囲は、式(１７)の算出結果は、「１」、すなわち正となる。
【０１４２】
（ａ８〜ａ９の方位角）
ａ８〜ａ９の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は環境Ｋの壁部分のものである。又、一方、移動後の全方位距離画像の距離値ｄg（θ，γ）も同じ環境Ｋの壁部分のものである。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＝０となる。この結果、方位角ａ８〜ａ９の範囲では、式(１７)の算出結果は、「０」となる。
【０１４３】
上記のように、差分画像において、アクティブセンサ１１に近い位置へ変化した領域は、「正」の領域となり、一方、遠い位置へ変化した領域は「負」の領域で現れる。どちらの領域においても、移動後に得られる距離画像から、動物体２０を抽出したことになる。すなわち、「正」の領域は、動物体２０の移動後を抽出したことになり、「負」の領域は、動物体２０の移動前を抽出したことになる。
【０１４４】
なお、オクルージョン領域を考慮しない場合、動物体２０が移動後に現れるオクルージョン領域(先の例では、ａ３〜ａ４の方位角の領域)も動物体と検出してしまうことになる。しかし、Ｓ３０において、オクルージョン領域を推定し、推定したオクルージョン領域を式(１７)にて除外して区別しているため、動物体領域のみを検出できる。
【０１４５】
２． 具体例２
次に図２１及び図２２で示す具体例２を参照して、式(１７)の判定値の設定について説明する。具体例２は、アクティブセンサ１１と、動物体２０とが、同一線上において、互いに接近移動するときの例である。
【０１４６】
なお、この例では、Ｓ３０ではオクルージョン領域は、下記のように推定されている。図２２では、ジャンプエッジの画素に対応する部分を(θ1，γ1，ｄs（θ1，γ1）)で示しており、その画素から最も遠い画素に相当する部位を(θ2，γ2，ｄs（θ2，γ2）)で示す。
【０１４７】
この場合、式(１４)により、アクティブセンサ１１の移動により、
(θ1，γ1，ｄs（θ1，γ1）) → (θ1’，γ1’，ｄp（θ1’，γ1’）)
(θ2，γ2，ｄs（θ2，γ2）) → (θ2’，γ2’，ｄp（θ2’，γ2’）)
のように変換されている。なお、説明の便宜上、仰角γは０としている。
【０１４８】
そして、この場合、図２２に示すように、θ1＞θ2であり、かつ、θ２’＞θ１’であるため、式(１５)により、オクルージョン区間Ｉθは空集合となり、オクルージョン領域Ｏｃ（θ，γ）＝０、すなわち、オクルージョン領域はないとされている。なお、図２２では、ジャンプエッジは、動物体２０の右側の部分としたが、左側にもジャンプエッジが存在する。しかし、前述した同じ理由により、こちらの側もオクルージョン領域はないとされる。
【０１４９】
次に、Ｓ４０における処理を説明する。
図２１は、アクティブセンサ１１の移動前後の位置と、動物体２０の移動前後の位置をそれぞれ示している。なお、説明の便宜上、以下は、アクティブセンサ１１において、仰角γ＝０における画素を前提として説明する。又、閾値Ｔｈを０とする。同図中、動物体２０、及びアクティブセンサ１１が移動後に実際に取得した全方位距離画像は、ａ１〜ａ２〜ａ３〜ａ４間の動物体２０(移動後)の円弧部分，ａ４〜ａ１間の環境Ｋの壁を含んでいる。
【０１５０】
又、Ｓ２０で算出した予測距離画像は、ａ１〜ａ２間の環境Ｋの壁、ａ２〜ａ３間の動物体２０(移動前)の円弧部分、ａ３〜ａ４〜ａ１の環境Ｋの壁を含んでいる。
【０１５１】
(ａ１〜ａ２の方位角)
さて、ａ１〜ａ２の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は環境Ｋの壁部分のものであり、一方、移動後の全方位距離画像の距離値ｄg（θ，γ）は動物体２０(移動後)の円弧部分のものである。従って、予測距離画像の距離値ｄp（θ，γ）の方が、移動後の全方位距離画像の距離値ｄg（θ，γ）のよりも大きくなる。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＞０(＝Ｔｈ)となる。又、この方位角ａ１〜ａ２は、Ｏ(θ，γ)＝０、すなわち、オクルージョン領域でない。この結果、方位角ａ１〜ａ２の範囲では、式(１７)の算出結果は、「１」、すなわち正となる。なお、図２１では、式(１７)の算出結果を括弧書で示している。
【０１５２】
(ａ２〜ａ３の方位角)
ａ２〜ａ３の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は移動前の動物体２０の円弧部分のものである。一方、移動後の全方位距離画像の距離値ｄg（θ，γ）はアクティブセンサ１１に接近移動後の動物体２０の円弧部分のものである。このため、距離値ｄp（θ，γ）＞距離値ｄg（θ，γ）となり、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＞０(＝Ｔｈ)となる。又、この方位角ａ２〜ａ３は、Ｏ(θ，γ)＝０、すなわち、オクルージョン領域でない。この結果、方位角ａ２〜ａ３の範囲では、式(１７)の算出結果は、「１」、すなわち正となる。
【０１５３】
(ａ３〜ａ４の方位角)
ａ３〜ａ４の方位角の範囲では、ａ１〜ａ２と同じ理由により、式(１７)の算出結果は、「１」、すなわち正となる。
【０１５４】
(ａ４〜ａ１の方位角)
ａ４〜ａ１の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）及び移動後の全方位距離画像の距離値ｄg（θ，γ）は同じ環境Ｋの壁部分のものである。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＝０となる。この結果、方位角ａ４〜ａ１の範囲では、式(１７)の算出結果は、「０」となる。
【０１５５】
このように、差分画像において、アクティブセンサ１１に近い位置へ変化した領域は、「正」の領域となり、移動後に得られる距離画像から、動物体２０を抽出したことになる。すなわち、「正」の領域は、動物体２０の移動後を抽出したことになる。
【０１５６】
３． 具体例３
次に図２３で示す具体例３を参照して、式(１７)の判定値の設定について説明する。具体例３は、アクティブセンサ１１と、動物体２０とが、同一線上において、同方向に移動するときの例である。この場合、具体例２と同じ理由で、Ｓ３０では、オクルージョン領域はないとされる。
【０１５７】
Ｓ４０における処理を説明する。
図２３は、アクティブセンサ１１の移動前後の位置と、動物体２０の移動前後の位置をそれぞれ示している。なお、説明の便宜上、以下は、アクティブセンサ１１において、仰角γ＝０における画素を前提として説明する。又、閾値Ｔｈを０とする。同図中、動物体２０、及びアクティブセンサ１１が移動後に実際に取得した全方位距離画像は、ａ１〜ａ２間の環境Ｋの壁、ａ２〜ａ３間の動物体２０(移動後)の円弧部分，ａ３〜ａ４〜ａ１の環境Ｋの壁を含んでいる。
【０１５８】
又、Ｓ２０で算出した予測距離画像は、ａ１〜ａ２間の動物体２０(移動前)の円弧部分、ａ２〜ａ３間の動物体２０(移動前)の円弧部分、ａ３〜ａ４間の動物体２０(移動前)の円弧部分、ａ４〜ａ１の環境Ｋの壁を含んでいる。
【０１５９】
(ａ１〜ａ２の方位角)
ａ１〜ａ２の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は動物体２０(移動前)の円弧部分のものであり、一方、移動後の全方位距離画像の距離値ｄg（θ，γ）は環境Ｋの壁のものである。従って、予測距離画像の距離値ｄp（θ，γ）の方が、移動後の全方位距離画像の距離値ｄg（θ，γ）のよりも小さくなる。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＜０(＝−Ｔｈ)となる。又、この方位角ａ１〜ａ２は、Ｏ(θ，γ)＝０、すなわち、オクルージョン領域でない。この結果、方位角ａ１〜ａ２の範囲では、式(１７)の算出結果は、「−１」、すなわち負となる。なお、図２３では、式(１７)の算出結果を括弧書で示している。
【０１６０】
(ａ２〜ａ３の方位角)
ａ２〜ａ３の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）は移動前の動物体２０の円弧部分のものである。一方、移動後の全方位距離画像の距離値ｄg（θ，γ）はアクティブセンサ１１から離間移動後の動物体２０の円弧部分のものである。このため、距離値ｄp（θ，γ）＜距離値ｄg（θ，γ）となり、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＜０(＝−Ｔｈ)となる。又、この方位角ａ２〜ａ３は、Ｏ(θ，γ)＝０、すなわち、オクルージョン領域でない。
【０１６１】
この結果、方位角ａ２〜ａ３の範囲では、式(１７)の算出結果は、「−１」、すなわち負となる。
(ａ３〜ａ４の方位角)
ａ３〜ａ４の方位角の範囲では、ａ１〜ａ２と同じ理由により、式(１７)の算出結果は、「−１」、すなわち負となる。
【０１６２】
(ａ４〜ａ１の方位角)
ａ４〜ａ１の方位角の範囲では、予測距離画像の距離値ｄp（θ，γ）及び移動後の全方位距離画像の距離値ｄg（θ，γ）は同じ環境Ｋの壁部分のものである。このため、式(１６)を算出すると、生成する差分画像の距離値ｄδ(θ，γ)＝０となる。この結果、方位角ａ４〜ａ１の範囲では、式(１７)の算出結果は、「０」となる。
【０１６３】
このように、差分画像において、アクティブセンサ１１から遠い位置へ変化した領域は、「負」の領域となり、移動後に得られる距離画像から、動物体２０を抽出したことになる。すなわち、「負」の領域は、動物体２０の移動前を抽出したことになる。
【０１６４】
アクティブセンサ１１と動物体２０とが同一直線上において、移動する場合、具体例２及び具体例３以外に、互いに離間する方向に移動する場合や、具体例３とは１８０度反対向きに、アクティブセンサ１１と動物体２０が移動する場合もある。これらの場合も、同様に、動物体２０の領域が「正」又は「負」となり、抽出される。
【０１６５】
さて具体例の説明を終了して、フローチャートの説明に戻る。
Ｓ４０では、前述のように処理した後、３値画像に対して、ノイズ除去処理を行い、Ｓ４０の処理を終了する。なお、ノイズ除去処理は、例えば、画素数（ピクセル数）が所定閾値以下のものを、ノイズとして除去する処理である。
【０１６６】
本実施形態によれば、下記に示す効果を有する。
（１） 本実施形態のアクティブセンサ１１の動物体検出装置では、コンピュータ１６(相対移動量推定手段)は、アクティブセンサ１１の移動前と移動後の、移動方向、移動距離、回転量を含む相対移動量を推定するようにした。又。コンピュータ１６(予測距離画像生成手段)は、移動前に取得した全方位距離画像と、前記推定した相対移動量に基づいて、移動後の予測距離画像を推定して生成するようにした。さらに、コンピュータ１６(オクルージョン領域推定手段)は、アクティブセンサ１１が移動することにより、前記予測距離画像に生ずるオクルージョン領域を推定するようにした。そして、コンピュータ１６(動物体領域抽出手段)は、前記予測距離画像と、移動後にアクティブセンサ１１が取得した全方位距離画像との差分画像を生成する際に、前記オクルージョン領域を取り除き、動物体領域を抽出するようにした。
【０１６７】
又、本実施形態のアクティブセンサの動物体検出プログラムは、コンピュータ１６を上記各手段として機能させるようにした。
又、アクティブセンサ１１の動物体検出方法は、第１ステップとして、アクティブセンサ１１の移動前後の、移動方向、移動距離、回転量を含む相対移動量を推定するようにした。又、第２ステップとして、移動前に取得した全方位距離画像と、前記第１ステップにて推定した相対移動量に基づいて、移動後の予測距離画像を推定して生成するようにした。さらに、第３ステップとして、アクティブセンサ１１が移動することにより、前記予測距離画像に生ずるオクルージョン領域を推定するようにした。又、第４ステップとして、前記予測距離画像と、移動後にアクティブセンサ１１が取得した全方位距離画像との差分画像を生成する際に、前記オクルージョン領域を取り除き、動物体領域を抽出するようにした。
【０１６８】
この結果、観測系が移動することにより生ずる動物体以外の環境の「見かけの変化」と、人物等の動物体による環境変化を容易に区別して動物体の検出を容易にでき、オクルージョン領域を推定して除去することにより、より正確に動物体による環境変化のみを検出することができる効果を奏する。
【０１６９】
（２） 本実施形態では、コンピュータ１６は、移動前の全方位距離画像のジャンプエッジ画素と、同画素の４−近傍（所定範囲内）に位置する画素の中で、アクティブセンサ１１から最も遠い距離値を有する対比対象画素について、相対移動量に基づき、それぞれ移動後の推定方位角、及び推定仰角を求めた。そして、コンピュータ１６は、ジャンプエッジ画素と対比対象画素のそれぞれの推定方位角の区間、及び推定仰角の範囲を、前記予測距離画像における前記オクルージョン領域のオクルージョン区間であると推定するようにした。
【０１７０】
又、本実施形態のアクティブセンサの動物体検出プログラムは、コンピュータ１６を上記手段として機能させるようにした。
又、本実施形態では、第３ステップとして、移動前の全方位距離画像のジャンプエッジ画素と、同画素の４−近傍（所定範囲内）に位置する画素の中で、アクティブセンサ１１から最も遠い距離値を有する対比対象画素について、相対移動量に基づき、それぞれ移動後の推定方位角、及び推定仰角を求めた。そして、第３ステップでは、ジャンプエッジ画素と対比対象画素のそれぞれの推定方位角の区間、及び推定仰角の範囲を、前記予測距離画像における前記オクルージョン領域のオクルージョン区間であると推定するようにした。
【０１７１】
この結果、オクルージョン領域を推定するためのオクルージョン区間を、容易に決定できる。
（３） 本実施形態では、コンピュータ１６は、オクルージョン領域推定手段として、判定対象画素の仰角が、ジャンプエッジ画素の推定仰角に一致し、かつ、判定対象画素の方位角が推定方位角で定めたオクルージョン区間内にあるときは、判定対象画素を、オクルージョン領域内にあると判定するようにした。
【０１７２】
又、本実施形態のアクティブセンサの動物体検出プログラムは、コンピュータ１６を上記手段として機能させるようにした。
又、本実施形態では、第３ステップとして、判定対象画素の仰角が、ジャンプエッジ画素の推定仰角に一致し、かつ、判定対象画素の方位角が推定方位角で定めたオクルージョン区間内にあるとき(条件１)は、判定対象画素は、オクルージョン領域内にあると判定するようにした。
【０１７３】
この結果、条件１が成立したときに、オクルージョン領域の推定を容易にできる。
（４） 又、コンピュータ１６は、オクルージョン領域推定手段として、判定対象画素の方位角が、ジャンプエッジ画素の推定方位角に一致し、かつ、判定対象画素の仰角が推定仰角で定めたオクルージョン区間内にあるとき(条件２)は、判定対象画素をオクルージョン領域内にあると判定するようにした。
【０１７４】
又、本実施形態のアクティブセンサの動物体検出プログラムは、コンピュータ１６を上記手段として機能させるようにした。
又、第３ステップとして、判定対象画素の方位角が、ジャンプエッジ画素の推定方位角に一致し、かつ、判定対象画素の仰角が推定仰角で定めたオクルージョン区間内にあるとき(条件２)は、判定対象画素をオクルージョン領域内にあると判定するようにした。
【０１７５】
この結果、条件２が成立したときに、オクルージョン領域の推定を容易にできる。
（５） 本実施形態では、コンピュータ１６は、動物体領域抽出手段として、差分画像を生成する際に、各画素の距離値の差に基づいて、正及び負の符号判定を行い、この正又は負の符号判定に基づいて、動物体領域抽出を行う。
【０１７６】
又、本実施形態のアクティブセンサの動物体検出プログラムは、コンピュータ１６を上記手段として機能させるようにした。
又、第４ステップとして、差分画像を生成する際に、各画素の距離値の差に基づいて、正及び負の符号判定を行い、この正及び負の符号判定に基づいて、動物体領域抽出を行う。
【０１７７】
この結果、正及び負の符号判定を行うことにより、移動前と移動後の両方の動物体領域抽出を容易に行うことができる。
(実験例)
図２４〜図３１は、実験室において、アクティブセンサ１１は、人物が位置する方向に向かって移動するとともに、動物体としての人物が実験室の壁側に向かって移動する前後の画像を取得し画像処理した結果を示している。なお、各図において、横軸は、方位角θ（０＜θ＜３６０度）であり、縦軸は、仰角γ(−３／３＜γ＜π／３)の範囲を対象として図示している。
【０１７８】
図２４は「移動前」、図２５は「移動後」のもので、両図は、距離画像ではなく、全方位のカラー画像で取得したものを説明の便宜上、白黒の濃淡画像で示している。図２６は図２４に対応した全方位距離画像、図２７は、図２５に対応した距離画像である。
【０１７９】
そして、得られた相対移動量を用いて、図２６から生成した予測距離画像を図２８に、推定したオクルージョン領域を、図２９に示している。図３０は、図２８の予測距離画像と移動後取得した全方位距離画像(図２７)の差分をとり、距離値が「正」であった領域を白、「負」であった領域をグレーで示している。ここでは、グレーで示された領域は、アクティブセンサ１１が移動前存在していたものが、移動後なくなっていることを示している。又、白で示す領域は、アクティブセンサ１１が移動前なかったものが、移動後現れた領域を示している。図２８において、人物像の右側輪郭に沿って現れている黒い領域は、オクルージョン領域推定に相当する。この領域は、図２９で示す、推定したオクルージョン領域にて取り除き、移動後現れた領域のみを図３１に示す。
【０１８０】
このように、オクルージョン領域を考慮することにより、動物体を精度良く検出できている。
なお、本発明の実施形態は上記実施形態に限定されるものではなく、発明の趣旨を逸脱しない範囲で、適宜に変更して次のように実施することもできる。
【０１８１】
（１） 前記実施形態では、アクティブセンサ１１として、「”実環境センシングのための全方位ステレオシステム（ＳＯＳ）”、電気学会論文誌C.Vol.121-C,No.5,pp.876-881.2001」に記載されているものを使用した。これに限らず、他の全方位カメラから得られた全方位距離画像を入力するようにしてもよい。
【０１８２】
(２) 前記実施形態では、コンピュータ１６は、動物体領域抽出手段として、差分画像を生成する際に、各画素の距離値の差に基づいて、正及び負の符号判定を行い、この正及び負の符号判定に基づいて、動物体領域抽出を行った。
【０１８３】
これに代えて、コンピュータ１６は、動物体領域抽出手段として、差分画像を生成する際に、各画素の距離値の差に基づいて、正又は負のいずれかの符号判定を行い、この正又は負のいずれかの符号判定に基づいて、動物体領域抽出を行うようにしてもよい。
【０１８４】
この場合、移動前、或いは、移動後の動物体領域抽出を行うことができる。
(３) 前記実施形態では、Ｓ３０において、移動前において取得した全方位距離画像におけるジャンプエッジの画素(ピクセル)を、(θ1，γ1，ｄs（θ1，γ1）)とする。そして、画素(θ1，γ1，ｄs（θ1，γ1）)の所定範囲内としての４−近傍にある距離値ｄs（θ1，γ1）より最も遠い距離値を有する画素を(θ2，γ2，ｄs（θ2，γ2）)とした。そして、「所定値」とは、最も遠い距離値を意味するようにしたが、これに限定するものではない。
【０１８５】
例えば、動物体の移動速度と、アクティブセンサの移動速度が予め分かっており、両者の移動範囲が分かっている場合には、ジャンプエッジと４−近傍にある画素との距離値がとりうる範囲を予め予測できる。この場合には、予測できる距離値の範囲の中で、所定値として予め定数を決定しておき、距離値がこの定数以上の複数の画素を対比対象画素としてもよい。
【０１８６】
(４) 前記実施形態では、ジャンプエッジ画素に対する所定範囲内として４−近傍としたが、８−近傍や１６−近傍等であってもよい。 (５) 前記実施形態では、式(１５)において、予測距離画像における判定対象画素の仰角γが、推定仰角γ1’に一致し、予測距離画像における判定対象画素の方位角θがオクルージョン区間Ｉθ内にあるときは、Ｏ（θ，γ）＝１とする。すなわち、判定対象画素はオクルージョン領域内にあると判定した。又、予測距離画像における判定対象画素の方位角θが推定方位角θ1’に一致し、判定対象画素の仰角γがオクルージョン区間Ｉγ内にあるときは、Ｏ（θ，γ）＝１とする。すなわち、判定対象画素はオクルージョン領域内にあると判定した。そうでない場合には、Ｏ（θ，γ）＝０とした。
【０１８７】
これに代えて、上記(３)の場合のように、対比対象画素が複数になった場合、前記複数の対比対象画素で囲まれる範囲をオクルージョン領域として判定してもよい。
【０１８８】
【発明の効果】
以上詳述したように請求項１乃至請求項１０に記載の発明によれば、観測系が移動することにより生ずる動物体以外の環境の「見かけの変化」と、人物等の動物体による環境変化を容易に区別して動物体の検出を容易にできる。そして、オクルージョン領域を推定して除去することにより、より正確に動物体による環境変化のみを検出することができる効果を奏する。
【０１８９】
請求項１１乃至請求項１５の発明によれば、アクティブセンサの動物体検出方法及び動物体検出装置を容易に実現できる動物体検出プログラムを提供できる。
【図面の簡単な説明】
【図１】 アクティブセンサ１１の電気的構成のブロック図。
【図２】 アクティブセンサ１１の機械的構成の概略図。
【図３】 （ａ）〜（ｃ）はコンピュータ１６が実行する位置・姿勢推定プログラムのフローチャート。
【図４】 図５に対応するパノラマエッジ画像の説明図。
【図５】 エッジヒストグラムの例を示す説明図。
【図６】 アクティブセンサ１１の平行移動におけるエッジヒストグラムのシフトを説明するための説明図。
【図７】 （ａ）はアクティブセンサ１１を中心にした円筒座標系の説明図、（ｂ）は、アクティブセンサ１１を中心にした四角筒座標系の説明図、（ｃ）は、異なる空間系にエッジを写像したときの、ヒストグラムのシフトとsin曲線を表した説明図。
【図８】 アクティブセンサ１１の２つの地点における全方位エッジヒストグラムのマッチングコストマトリクスＣ（s,i）の説明図。
【図９】 総合シフト量から回転量ｓφと移動方向ωを求める方法の説明図。
【図１０】 床平面投影ヒストグラムの説明図。
【図１１】 本実施形態において、ρ−θ表面上で複数の候補点の各々に退位する直線群を示した図。
【図１２】 ドミナント射影方向のヒストグラム。
【図１３】 図１２とは異なるドミナント射影方向のヒストグラム。
【図１４】ハフ変換の説明図。
【図１５】ρ−θ表面上で、ある点を通る直線群を示した図。
【図１６】ρ−θ表面上で複数の候補点の各々に退位する直線群を示した図。
【図１７】仰角γの説明図。
【図１８】距離値の説明図。
【図１９】オクルージョン領域の説明図。
【図２０】具体例１の式(１７)の判定値の設定の説明図。
【図２１】具体例２の式(１７)の判定値の設定の説明図。
【図２２】同じく具体例２のジャンプエッジの画素に関する説明図。
【図２３】具体例３の式(１７)の判定値の設定の説明図。
【図２４】 アクティブセンサ１１の「移動前」における白黒の濃淡画像。
【図２５】 アクティブセンサ１１の「移動後」における白黒の濃淡画像。
【図２６】図２４に対応した全方位距離画像。
【図２７】図２５に対応した距離画像。
【図２８】予測距離画像。
【図２９】推定して生成されたオクルージョン領域を示す画像。
【図３０】差分画像。
【図３１】動物体移動後の動体検出された画像。
【符号の説明】
１１…アクティブセンサ
１２…ステレオユニット
１６…コンピュータ（相対移動量推定手段、予測距離画像生成手段、オクルージョン領域推定手段、及び動物体領域抽出手段）
Claims (15)
- 全方位距離画像を取得可能であり、環境内を移動可能なアクティブセンサの動物体検出装置において、
前記アクティブセンサの移動前と移動後の、移動方向、移動距離、回転量を含む相対移動量を推定する相対移動量推定手段と、
移動前に取得した全方位距離画像と、前記相対移動量推定手段にて推定した相対移動量に基づいて、移動後の全方位距離画像(以下、予測距離画像という)を推定して生成する予測距離画像生成手段と、
前記アクティブセンサが移動することにより、前記予測距離画像に生ずるオクルージョン領域を推定するオクルージョン領域推定手段と、
前記予測距離画像と、移動後にアクティブセンサが取得した全方位距離画像との差分画像を生成する際に、前記オクルージョン領域を取り除き、動物体領域を抽出する動物体領域抽出手段とを備えたことを特徴とするアクティブセンサの動物体検出装置。 - 前記オクルージョン領域推定手段は、
移動前に取得した全方位距離画像のジャンプエッジの画素(以下、ジャンプエッジ画素という)と、同画素の所定範囲内に位置する画素の中で、アクティブセンサから所定値以上の距離値を有する画素(以下、対比対象画素という)について、前記相対移動量に基づき、それぞれ移動後の推定方位角、及び推定仰角を求め、
前記ジャンプエッジ画素と対比対象画素のそれぞれの推定方位角の区間、及び推定仰角の範囲を、前記予測距離画像における前記オクルージョン領域のオクルージョン区間であると推定することを特徴とする請求項１に記載のアクティブセンサの動物体検出装置。 - 前記オクルージョン領域推定手段は、
前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の仰角が、前記ジャンプエッジ画素の推定仰角に一致し、かつ、前記判定対象画素の方位角が前記推定方位角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定することを特徴とする請求項２に記載のアクティブセンサの動物体検出装置。 - 前記オクルージョン領域推定手段は、
前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の方位角が、前記ジャンプエッジ画素の推定方位角に一致し、かつ、前記判定対象画素の仰角が前記推定仰角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定することを特徴とする請求項２に記載のアクティブセンサの動物体検出装置。 - 前記動物体領域抽出手段は、
前記差分画像を生成する際に、各画素の距離値の差に基づいて、動物体領抽出を行うことを特徴とする請求項１乃至請求項４のうちいずれか１項に記載のアクティブセンサの動物体検出装置。 - 全方位距離画像を取得可能であり、環境内を移動可能なアクティブセンサの動物体検出方法において、
前記アクティブセンサの移動前と移動後の、移動方向、移動距離、回転量を含む相対移動量を推定する第１ステップと、
移動前に取得した全方位距離画像と、前記第１ステップにて推定した相対移動量に基づいて、移動後の全方位距離画像(以下、予測距離画像という)を推定して生成する第２ステップと、
前記アクティブセンサが移動することにより、前記予測距離画像に生ずるオクルージョン領域を推定する第３ステップと、
前記予測距離画像と、移動後にアクティブセンサが取得した全方位距離画像との差分画像を生成する際に、前記オクルージョン領域を取り除き、動物体領域を抽出する第４ステップを含むことを特徴とするアクティブセンサの動物体検出方法。 - 前記第３ステップは、
移動前に取得した全方位距離画像のジャンプエッジの画素(以下、ジャンプエッジ画素という)と、同画素の所定範囲内に位置する画素の中で、アクティブセンサから所定値以上の距離値を有する画素(以下、対比対象画素という)について、前記相対移動量に基づき、それぞれ移動後の推定方位角、及び推定仰角を求め、
前記ジャンプエッジ画素と対比対象画素のそれぞれの推定方位角の区間、及び推定仰角の範囲を、前記予測距離画像における前記オクルージョン領域のオクルージョン区間であると推定することを含むことを特徴とする請求項６に記載のアクティブセンサの動物体検出方法。 - 前記第３ステップは、
前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の仰角が、前記ジャンプエッジ画素の推定仰角に一致し、かつ、前記判定対象画素の方位角が前記推定方位角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定することを特徴とする請求項７に記載のアクティブセンサの動物体検出方法。 - 前記第３ステップは、
前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の方位角が、前記ジャンプエッジ画素の推定方位角に一致し、かつ、前記判定対象画素の仰角が前記推定仰角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定することを特徴とする請求項７に記載のアクティブセンサの動物体検出方法。 - 前記第４ステップは、
前記差分画像を生成する際に、各画素の距離値の差に基づいて、動物体領域抽出を行うことを特徴とする請求項６乃至請求項９のうちいずれか１項に記載のアクティブセンサの動物体検出方法。 - コンピュータを、
アクティブセンサの移動前と移動後の、移動方向、移動距離、回転量を含む相対移動量を推定する相対移動量推定手段と、
移動前に取得した全方位距離画像と、前記相対移動量推定手段にて推定した相対移動量に基づいて、移動後の全方位距離画像(以下、予測距離画像という)を推定して生成する予測距離画像生成手段と、
前記アクティブセンサが移動することにより、前記予測距離画像に生ずるオクルージョン領域を推定するオクルージョン領域推定手段と、
前記予測距離画像と、移動後にアクティブセンサが取得した全方位距離画像との差分画像を生成する際に、前記オクルージョン領域を取り除き、動物体領域を抽出する動物体領域抽出手段として、機能させることを特徴とするアクティブセンサの動物体検出プログラム。 - 請求項１１において、
さらに、コンピュータを、
前記オクルージョン領域推定手段として、
移動前に取得した全方位距離画像のジャンプエッジの画素(以下、ジャンプエッジ画素という)と、同画素の所定範囲内に位置する画素の中で、アクティブセンサから所定値以上の距離値を有する画素(以下、対比対象画素という)について、前記相対移動量に基づき、それぞれ移動後の推定方位角、及び推定仰角を求め、
前記ジャンプエッジ画素と対比対象画素のそれぞれの推定方位角の区間、及び推定仰角の範囲を、前記予測距離画像における前記オクルージョン領域のオクルージョン区間であると推定するように機能させることを特徴とするアクティブセンサの動物体検出プログラム。 - 請求項１２において、
さらに、コンピュータを、
前記オクルージョン領域推定手段として、
前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の仰角が、前記ジャンプエッジ画素の推定仰角に一致し、かつ、前記判定対象画素の方位角が前記推定方位角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定するように機能させることを特徴とするアクティブセンサの動物体検出プログラム。 - 請求項１２において、
さらに、コンピュータを、
前記オクルージョン領域推定手段として、
前記予測距離画像における判定対象画素を、前記オクルージョン領域にあるか否かを判定する際に、前記判定対象画素の方位角が、前記ジャンプエッジ画素の推定方位角に一致し、かつ、前記判定対象画素の仰角が前記推定仰角で定めたオクルージョン区間内にあるときは、前記判定対象画素は、オクルージョン領域内にあると判定するように機能させることを特徴とするアクティブセンサの動物体検出プログラム。 - 請求項１１乃至請求項１４のうちいずれか１項において、
さらに、コンピュータを
前記動物体領域抽出手段として、
前記差分画像を生成する際に、各画素の距離値の差に基づいて、動物体領域抽出を行うことを特徴とするアクティブセンサの動物体検出プログラム。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2003162834A JP4316936B2 (ja) | 2003-06-06 | 2003-06-06 | アクティブセンサの動物体検出装置及び動物体検出方法、並びに動物体検出プログラム |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2003162834A JP4316936B2 (ja) | 2003-06-06 | 2003-06-06 | アクティブセンサの動物体検出装置及び動物体検出方法、並びに動物体検出プログラム |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2004362469A JP2004362469A (ja) | 2004-12-24 |
| JP4316936B2 true JP4316936B2 (ja) | 2009-08-19 |
Family
ID=34054864
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2003162834A Expired - Fee Related JP4316936B2 (ja) | 2003-06-06 | 2003-06-06 | アクティブセンサの動物体検出装置及び動物体検出方法、並びに動物体検出プログラム |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP4316936B2 (ja) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5302730B2 (ja) * | 2009-03-25 | 2013-10-02 | トヨタ自動車株式会社 | 運転者監視装置 |
| JP5051670B2 (ja) * | 2010-02-15 | 2012-10-17 | Ｎｅｃシステムテクノロジー株式会社 | 画像処理装置、画像処理方法および画像処理プログラム |
| US11300663B2 (en) | 2016-03-31 | 2022-04-12 | Nec Corporation | Method for predicting a motion of an object |
| US20190188631A1 (en) * | 2017-12-20 | 2019-06-20 | Tyco Fire & Security Gmbh | Systems and methods for multi-sensor tag sale optimization |
| JP7217741B2 (ja) * | 2018-04-05 | 2023-02-03 | 株式会社小糸製作所 | 演算処理装置、オブジェクト識別システム、オブジェクト識別方法、自動車、車両用灯具 |
| CN115096241B (zh) * | 2022-07-27 | 2023-11-10 | 无锡苏广汽车部件科技有限公司 | 一种汽车部件快速智能检测方法及其检测系统 |
-
2003
- 2003-06-06 JP JP2003162834A patent/JP4316936B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JP2004362469A (ja) | 2004-12-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10133279B2 (en) | Apparatus of updating key frame of mobile robot and method thereof | |
| JP5328979B2 (ja) | 物体認識方法、物体認識装置、自律移動ロボット | |
| EP3084719B1 (en) | Slam on a mobile device | |
| KR101784183B1 (ko) | ADoG 기반 특징점을 이용한 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법 | |
| Schoenberg et al. | Segmentation of dense range information in complex urban scenes | |
| US8824775B2 (en) | Robot and control method thereof | |
| CN108481327B (zh) | 一种增强视觉的定位装置、定位方法及机器人 | |
| KR20150144731A (ko) | 다이렉트 트래킹을 이용하여 이동 로봇의 위치를 인식하기 위한 장치 및 그 방법 | |
| WO2014114923A1 (en) | A method of detecting structural parts of a scene | |
| WO2008079541A9 (en) | Human pose estimation and tracking using label | |
| US20140226895A1 (en) | Feature Point Based Robust Three-Dimensional Rigid Body Registration | |
| EP2610783B1 (en) | Object recognition method using an object descriptor | |
| Maier et al. | Vision-based humanoid navigation using self-supervised obstacle detection | |
| KR101478709B1 (ko) | Rgb-d 영상 특징점 추출 및 특징 기술자 생성 방법 및 장치 | |
| US11729367B2 (en) | Wide viewing angle stereo camera apparatus and depth image processing method using the same | |
| EP3639192A1 (en) | Computer vision-based thin object detection | |
| JP6130325B2 (ja) | 道路環境認識装置 | |
| CN113267761A (zh) | 激光雷达目标检测识别方法、系统及计算机可读存储介质 | |
| JP6410231B2 (ja) | 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム | |
| Zhou et al. | Online multiple targets detection and tracking from mobile robot in cluttered indoor environments with depth camera | |
| JP4316936B2 (ja) | アクティブセンサの動物体検出装置及び動物体検出方法、並びに動物体検出プログラム | |
| Zhang et al. | Real-time obstacle detection based on stereo vision for automotive applications | |
| JP6886136B2 (ja) | 位置合わせ装置、位置合わせ方法及び位置合わせ用コンピュータプログラム | |
| US20200003574A1 (en) | Method and device for fast detection of repetitive structures in the image of a road scene | |
| Onoguchi et al. | Planar projection stereopsis method for road extraction |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20060605 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20090408 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20090421 |
|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20090521 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120529 Year of fee payment: 3 |
|
| LAPS | Cancellation because of no payment of annual fees |