JP4171282B2 - 地形認識装置および地形認識方法 - Google Patents

Description

【０００１】
本発明は、地面の三次元的形状を認識する地形認識装置および地形認識方法に係り、特に、移動体に搭載されたステレオカメラによって得られたステレオ画像に基づいて、地面の起伏形状を認識する技術に関する。
【０００２】
【従来の技術】
従来より、ステレオ画像を用いて、地面の三次元的形状を認識する技術が提案されている。例えば、特許文献１には、ステレオ画像を処理対象としたステレオマッチングを行うことで道路上の白線を検出し、検出された白線の三次元位置より道路面を検出する技術が開示されている。また、特許文献２には、ステレオ画像を用いて、道路や床のように画像的な特徴が少ない地面を大局的に平面近似することにより、地面を平面として認識する技術が開示されている。
【０００３】
また、特許文献３には、地面を含む景色を撮像することによって得られるステレオ画像に基づいて、地面上の障害物の有無に拘わらず、地面の起伏形状を認識する技術が開示されている。具体的には、まず、被写体内における三次元点の標高成分に基づいて、被写体の基準標高値が算出される。つぎに、この基準標高値を超える標高成分（突出標高）を有する三次元点が、突出位置として検出される。検出された突出位置は、三次元空間上において、突出した障害物が存在する位置とみなされる。この突出位置の標高成分に関しては、突出標高に代えて、突出位置の周囲の標高成分より算出された平均的標高値が用いられる。このように、障害物が存在する突出位置の標高成分を修正することによって、障害物を除いた地面の起伏形状が特定される。
【０００４】
なお、本願出願人による先行出願のうち、本発明と関連するものとして、特願２００２−１８４０２０号が存在する。
【０００５】
一方、特許文献４には、移動体にステレオカメラを２つ搭載し、それぞれより得られるステレオ画像を用いて、移動体の自己位置を認識する技術が開示されている。一方のステレオカメラは、遠方風景を撮像することによってステレオ画像（遠方画像）を時系列的なフレーム単位で出力するとともに、他方のステレオカメラは、下方風景を撮像することによってステレオ画像（下方画像）を時系列的なフレーム単位で出力する。移動体の運動は、フレーム間における遠方画像の動きと、フレーム間における下方画像の動きとに基づき検出される。遠方画像の動きおよび下方画像の動きは、周知の座標変換式より、それぞれの距離画像に基づいて三次元空間（実空間）における移動量に換算される。そして、下方画像の動きによる速度成分から遠方画像の動きによる回転速度成分を除去することによって、純並進速度成分が算出される。移動体の自己位置は、測距開始地点を基準とした並進速度成分に変換後、この成分を累積していくことによって、特定される。
【０００６】
【特許文献１】
特開平５−２６５５４７号公報
【特許文献２】
特開平９−８１７５５号公報
【特許文献３】
特開平８−２８５５８６号公報
【特許文献４】
特開平１１−５１６５０号公報
【０００７】
【発明が解決しようとする課題】
本発明の目的は、移動体に搭載されたステレオカメラを用いて、地面の三次元的形状を精度よく認識し、かつ、この地形認識を移動体の移動に伴い広範囲に亘って行うことである。
【０００８】
また、本発明の別の目的は、移動体に搭載されたステレオカメラを用いて、移動体の移動量を精度よく検出することである。
【０００９】
【課題を解決するための手段】
かかる課題を解決するために、第１の発明は、ステレオカメラと、ステレオ画像処理部と、地形データ生成部と、自己運動検出部と、地形マップ生成部とを有し、移動体に搭載され、地面の三次元的形状を認識する地形認識装置を提供する。ステレオカメラは、地面を含む景色を時系列的に撮像し、一対の画像データをフレーム毎に出力する。ステレオ画像処理部は、一対の画像データに基づき、ステレオマッチングによって視差を算出するとともに、算出された視差と、画像データにより規定される画像平面上の位置とが対応付けられた距離データをフレーム毎に出力する。地形データ生成部は、第１のフレームの距離データに基づいて、第１のフレームにおける地形の三次元的形状を示す地形データを生成する。自己運動検出部は、第１のフレームの画像データと、第１のフレームよりも前の第２のフレームの画像データと、第１のフレームの距離データと、第２のフレームの距離データとに基づいて、第１のフレームと第２のフレームとの間における移動体の移動量を検出する。地形マップ生成部は、自己運動検出部によって検出された移動量を考慮した上で、地形データ生成部によって生成された地形データを順次合成することにより、地形マップを生成する。また、地形データ生成部は、三次元位置算出部と、視線設定部と、視線ヒストグラム生成部と、地形認識部とを有する。三次元位置算出部は、第１の距離データに基づいて、それぞれの対象物の三次元位置を算出する。視線設定部は、三次元空間上において、対象物のそれぞれの三次元位置とステレオカメラの取付位置とを結ぶ視線群を設定する。視線ヒストグラム生成部は、高さ方向を除く二方向で、三次元空間を行列状に分割することによって、三次元空間上に複数の区分を設定する。そして、視線ヒストグラム生成部は、視線設定部によって設定された視線群に基づいて、区分毎に、高さ方向における視線通過の頻度分布を生成する。地形認識部は、視線通過の頻度分布に基づいて、区分のそれぞれにおける地面の高さを特定する。
【００１０】
第１の発明において、地形データ生成部は、対象物のそれぞれの三次元位置に基づいて、区分毎に、高さ方向における対象物の存在頻度分布を生成する対象物ヒストグラム生成部をさらに有していてもよい。この場合、地形認識部は、視線通過の頻度分布と対象物の存在頻度分布とに基づいて、区分のそれぞれにおける地面の高さを特定することが好ましい。さらに、地形認識部は、ある区分に関して特定された地面の高さが、区分の周囲の地面の高さよりも所定の閾値以上異なる場合、区分の地面の高さを、区分の周囲の地面の高さに基づいて、補正することが好ましい。
【００１１】
また、第１の発明において、自己運動量検出部は、第１のフレームまたは第２のフレームの一方の画像データによって規定される画像平面上に複数の基準領域を設定し、画像平面上における基準領域の位置を示す基準点を特定する基準領域設定部と、第１のフレームまたは第２のフレームの他方の画像データによって規定される画像平面上に、基準領域のそれぞれと相関を有する対応領域を設定し、画像平面上における対応領域の位置を示す対応点を特定する対応領域設定部と、基準領域設定部によって特定された複数の基準点のうち、３つの基準点を選択し、選択された基準点のそれぞれの三次元位置を、一方のフレームの距離データに基づいて算出するとともに、対応領域設定部によって特定された複数の対応点のうち、３つの対応点を選択し、選択された対応点のそれぞれの三次元位置を、他方のフレームの距離データに基づいて算出する三次元位置算出部と、三次元位置算出部によって算出された基準点の三次元位置と対応点の三次元位置とに基づいて、第１のフレームと第２のフレームとの間における移動体の移動量を検出する移動量検出部とを有していてもよい。この場合、三次元位置算出部は、選択された基準点の位置的関係の評価を行った上で、選択された基準点の三次元位置を算出することが好ましい。
【００１２】
第２の発明は、移動体に搭載され、地面の三次元的形状を認識する地形認識方法を提供する。この地形認識方法は、ステレオカメラによって、地面を含む景色を時系列的に撮像し、一対の画像データをフレーム毎に出力するステップと、一対の画像データに基づき、ステレオマッチングによって視差を算出するとともに、算出された視差と、画像データにより規定される画像平面上の位置とが対応付けられた距離データをフレーム毎に出力するステップと、第１のフレームの距離データに基づいて、第１のフレームにおける地形の三次元的形状を示す地形データを生成するステップと、第１のフレームの画像データと、第１のフレームよりも前の第２のフレームの画像データと、第１のフレームの距離データと、第２のフレームの距離データとに基づいて、第１のフレームと第２のフレームとの間における移動体の移動量を検出するステップと、検出された移動量を考慮した上で、フレーム毎に生成された地形データを順次合成することにより、地形マップを生成するステップとを有する。ここで、地形データを生成するステップは、第１の距離データに基づいて、それぞれの対象物の三次元位置を算出するステップと、三次元空間上において、対象物のそれぞれの三次元位置とステレオカメラの取付位置とを結ぶ視線群を設定するステップと、高さ方向を除く二方向で、三次元空間を行列状に分割することによって、三次元空間上に複数の区分を設定するとともに、設定された視線群に基づいて、区分毎に、高さ方向における視線通過の頻度分布を生成するステップと、視線通過の頻度分布に基づいて、区分のそれぞれにおける地面の高さを特定するステップとを有する。
【００１４】
【発明の実施の形態】
（第１の実施形態）
図１は、第１の実施形態にかかる地形認識装置のブロック構成図である。この地形認識装置１は、地面上に障害物が存在するか否かに拘わらず、起伏のある地面の三次元的形状を認識する。ステレオカメラ２は、地面を移動する移動体（農林作業車両、土木作業車両、或いは、探査車両等）に載置されており、地面を含む景色を所定の俯角付で時系列的に撮像する。ステレオカメラ２は、一対のカメラ２ａ，２ｂで構成されており、後段のステレオ画像処理を行うのに必要な一対の画像（ステレオ画像）をフレーム単位で出力する。それぞれのカメラ２ａ，２ｂには、ＣＣＤやＣＭＯＳセンサ等のイメージセンサが内蔵されている。メインカメラ２ａは基準画像（右画像）を撮像し、サブカメラ２ｂは比較画像（左画像）を撮像する。互いの同期が取れている状態において、カメラ２ａ，２ｂから出力されたアナログ画像は、Ａ／Ｄコンバータ４，５によって、所定の輝度階調（例えば、256階調のグレースケール）のデジタル画像に変換される。
【００１５】
ステレオカメラ２よりフレーム単位で出力されたステレオ画像は、画像補正部６において、輝度の補正や座標変換といった画像処理が行われる。通常、一対のカメラ２ａ，２ｂの取付位置は、程度の差はあるものの誤差が存在するため、それに起因したずれが左右の各画像に生じている。このようなずれを画像処理によって等価的に補正すべく、画像補正部６は、画像の回転や平行移動といった幾何学的な変換を行う。また、カメラ２ａ，２ｂの光学的な歪みも、画像補正部６での画像処理によって等価的に補正される。
【００１６】
このような画像処理を経て、メインカメラ２ａより基準画像データが得られ、サブカメラ２ｂより比較画像データが得られる。それぞれの画像データは、画素の輝度値（0〜255）の集合である。ここで、画像データによって規定される画像平面は、ｉ−ｊ座標系で表現され、画像の左下隅を原点として、水平方向をｉ軸、垂直方向をｊ軸とする。１画像の表示単位である１フレーム相当のステレオ画像データは、後段のステレオ画像処理部７に出力されるとともに、画像データメモリ９に格納される。
【００１７】
なお、画像データメモリ９は、後述するように、フレーム間における画像の動きに基づいて移動体の自己位置を検出する関係上、少なくとも２フレーム分のステレオ画像データＰn，Ｐn-1を格納可能な記憶容量を有する。以下、今回の撮像タイミングにて得られた１フレーム分の基準画像データＰnを適宜「第１のフレームの画像データＰn」と称し、それよりも前（典型的には直前）の撮像タイミングにて得られた１フレーム分の基準画像データＰn-1を適宜「第２のフレームの画像データＰn-1」と称する。
【００１８】
ステレオ画像処理部７は、基準画像データと比較画像データとに基づき、ステレオマッチング処理によって距離データを算出する。ここで、「距離データ」とは、基準画像データによって規定される画像平面において小領域毎に算出された視差ｄの集合であり、それぞれの視差ｄは画像平面上の位置（ｉ，ｊ）と対応付けられている。１つの視差ｄは、基準画像の一部を構成する所定面積（例えば、4×4画素）の画素ブロックより算出されるため、画素ブロックが視差ｄの算出単位となる。
【００１９】
図２は、基準画像に設定される画素ブロックの説明図である。例えば、基準画像が200×512画素で構成されている場合、１フレーム相当の撮像画像から、画素ブロックＰＢijの個数相当（50×128個）の視差群が算出され得る。周知のように、視差ｄは、その算出単位である画素ブロックＰＢijの水平方向のずれ量であり、画素ブロックＰＢij内に写し出された対象物までの距離と大きな相関がある。すなわち、画素ブロックＰＢij内に写し出されている対象物がステレオカメラ２に近いほど、この画素ブロックＰＢijの視差ｄは大きくなり、対象物が遠いほど視差ｄは小さくなる（対象物が無限に遠い場合、視差ｄは０になる）。
【００２０】
ある画素ブロックＰＢij（相関元）の視差ｄを算出する場合、この画素ブロックＰＢijの輝度特性と相関を有する領域（相関先）が比較画像において特定される。上述したように、ステレオカメラ２から対象物までの距離は、基準画像と比較画像との間における水平方向のずれ量に反映される。したがって、比較画像において相関先を探索する場合、比較画像の全体を探索する必要はなく、相関元となる画素ブロックＰijのｊ座標と同じ水平線（エピポーラライン）上を探索すればよい。ステレオ画像処理部７は、相関元のｉ座標を基準に設定された所定の探索範囲内において、エピポーラライン上を一画素ずつシフトしながら、相関元と相関先の候補との間における相関性を順次評価する（ステレオマッチング）。そして、原則として、最も相関が高いと判断される相関先（相関先の候補のうちのいずれか）の水平方向のずれ量が、その画素ブロックＰＢijの視差ｄとなる。
【００２１】
２つの画素ブロックの相関は、例えば、シティブロック距離ＣＢを算出することによって評価することができる。数式１は、シティブロック距離ＣＢの基本形を示す。同数式において、ｐ１ijは一方の画素ブロックのｉｊ番目の画素の輝度値であり、ｐ２ijは他方の画素ブロックのｉｊ番目の輝度値である。シティブロック距離ＣＢは、位置的に対応した２つの輝度値ｐ１ij，ｐ２ijの差（絶対値）の画素ブロック全体における総和であって、その差が小さいほど両画素ブロックの相関が大きいことを意味する。
【数１】
ＣＢ＝Σ｜ｐ１ij−ｐ２ij｜
【００２２】
基本的に、エピポーラライン上に存在する画素ブロック毎に算出されたシティブロック距離ＣＢのうち、その値が最小となる画素ブロックが相関先と判断される。そして、このようにして特定された相関先と相関元との間のずれ量が視差ｄとなる。なお、シティブロック距離ＣＢを算出するステレオ画像処理部７のハードウェア構成については、特開平５−１１４０９９号公報に開示されているので、必要ならば参照されたい。以上のような処理を経て算出された距離データ（ｉ，ｊ，ｄ）は、距離データメモリ８に格納される。
【００２３】
なお、距離データメモリ８は、後述するように、フレーム間における対象物の三次元位置の動きに基づいて移動体の自己位置を検出する関係上、少なくとも２フレーム分のステレオ距離データＤn，Ｄn-1を格納可能な記憶容量を有する。以下、第１のフレームの画像データＰnより算出された距離データＤnを適宜「第１のフレームの距離データＤn」と称し、第２のフレームの画像データＤn-1より算出された距離データＤn-1を適宜「第２のフレームの距離データＤn-1」と称する。
【００２４】
マイクロコンピュータ１０は、ＣＰＵ、ＲＯＭ、ＲＡＭ、入出力インターフェース等で構成されているが、これを機能的に捉えた場合、地形データ生成部１２、自己運動検出部１３および地形マップ生成部１４を有する。個々の機能ブロック１２〜１４の処理内容については後述するが、地形の三次元的形状を表すマップデータは、距離データメモリ８に格納された複数フレームの距離データと、画像データメモリ９に格納された複数フレームの画像データとに基づいて生成され、時系列的に随時更新される。
【００２５】
地形データ生成部１２は、第１のフレームの距離データＤnに基づいて、第１のフレームにおける地形データＧnを生成・出力する。地形データＧnは、第１のフレームにおいて、ステレオカメラ２の視野範囲内に写し出された地形の三次元的形状を表すデータである。図３は、地形データ生成部１２のブロック構成図である。地形データ生成部１２は、三次元位置算出部２０、視線設定部２１、視線ヒストグラム生成部２２および地形認識部２３とで構成されている。
【００２６】
三次元位置算出部２０は、距離データメモリ８に格納されている第１のフレームの距離データ（ｉ，ｊ，ｄ）を読み出し、それぞれの対象物の三次元位置（Ｘ，Ｙ，Ｚ）を算出する。三次元空間における対象物の位置を示す三次元位置（Ｘ，Ｙ，Ｚ）は、メインカメラ２ａの直下を基準とした場合、数式２に示すような周知の座標変換式に基づいて、距離データ（ｉ，ｊ，ｄ）より一義的に特定される。三次元位置は、Ｘ−Ｙ−Ｚ座標系で表現され、ステレオカメラ２（本実施形態ではメインカメラ２ａ）の中央真下の地面を原点Ｏとして、左右方向（カメラ基線方向）をＸ軸、上下方向（高さ方向）をＹ軸、前後方向（距離方向）をＺ軸とする。同数式において、定数ＫＺＨは、（カメラ基線長／１画素当たりの水平視野角）である。また、定数ＣＡＨはステレオカメラの取付高さ、定数ＰＷＶは１画素当たりの垂直視野角、定数ＰＷＨは１画素当たりの水平視野角である。さらに、定数ＩＶは予め設定された消失点Ｖのｉ座標値、定数ＪＶはそのｊ座標値である。
【数２】

【００２７】
視線設定部２１は、三次元位置算出部２０によって算出された三次元位置（Ｘ，Ｙ，Ｚ）に基づいて、三次元空間におけるステレオカメラ２の取付位置と対象物の位置とを結ぶ視線Ｌを設定する。図４は、Ｘ軸、Ｙ軸およびＺ軸によって規定される三次元空間において設定される視線Ｌの説明図である。ある対象物Ｂ（地面および障害物の双方を含む）に関する視線Ｌは、その対象物Ｂの三次元位置（Ｘ，Ｙ，Ｚ）とステレオカメラ２の取付位置Ｃ（０，ｙ１，０）とを通る直線式で規定され、この式によって、対象物Ｂまでの視線Ｌの通過経路が一義的に特定される。ここで、ｙ1は、ステレオカメラ２直下の地面を基準としたステレオカメラ２の取付高さである。視線設定部２１は、すべての対象物Ｂを演算対象として、それぞれの対象物Ｂに関する視線Ｌを順次算出する。１回の処理サイクルにおいて算出される視線Ｌ群の個数は、対象物Ｂの個数相当、換言すれば、１フレームの距離データを構成する視差ｄ群の個数相当である。
【００２８】
視線ヒストグラム生成部２２は、地面高の算出単位となる区分Ｓnmを三次元空間上に設定する。図５は三次元空間上に設定される区分Ｓnmの説明図である。三次元空間は、高さ方向を除く二方向で行列状、すなわち、Ｘ方向にＮ個、Ｚ方向にＭ個にそれぞれ分割される。これにより、三次元空間にＮ×Ｍ個の四角柱状の区分Ｓnmが定義される。地形マップの分解能は、１つの区分Ｓnmの底面積（横断面積）の大きさに依存しており、これが小さいほど地形マップの分解能（上面視の分解能）が向上する。ただし、区分Ｓnmの底面積が小さすぎると、地面高の算出精度が低下してしまうおそれがある。なぜなら、視線Ｌ群の統計処理よって地面高を決定する関係上、区分Ｓnmの底面積が小さくなると、１つの区分Ｓnm内を通過する視線Ｌの個数（サンプル数）が減少してしまうからである。したがって、区分Ｓnmの底面積（換言すれば、三次元空間の分割数）は、地形マップに要求される分解能と、地面高の算出精度との双方を考慮した上で、決定する必要がある。本実施形態では、１個当たりの区分Ｓnmの底形状を、一例として、20cm四方に設定している。このようにして、三次元空間を高さ方向を除く二方向で行列状に分割することによって、三次元空間上に複数の立体的な区分Ｓnmが格子状に設定される。１つの区分Ｓnm内には、例えば、図６に示すような通過経路で視線Ｌが存在する。ある区分Ｓnm内を多数の視線Ｌが通過するということは、その区分Ｓnm内に視線を遮る物体（障害物）が存在しないことを意味する。
【００２９】
なお、三次元空間を格子状に分割することにより四角柱状の区分Ｓnmを設定するのは一例であって、区分Ｓnmの設定手法や立体形状は、これに限定されるものではない。例えば、区分Ｓnmを六角柱状にしてもよい。
【００３０】
視線ヒストグラム生成部２２は、視線設定部２１によって設定された視線Ｌ群に基づいて、高さ方向における視線通過の頻度分布を生成する。この頻度分布の生成は、区分Ｓnm毎に行われる。図７は、ある区分Ｓnmに関する視線通過頻度ヒストグラムの一例を示す図である。同図に示すように、視線通過の頻度分布は、例えば、高さｙを縦軸として、予め設定された高さ区間（例えば、0.1ｍ）毎に縦軸が区切られている。地形マップの高さ方向の分解能は、１区間当たりの高さ区間の幅に依存している。そして、１つの視線Ｌがある高さ区間を通過する毎、その区間の度数に１を加算する。このような視線Ｌの通過回数の加算は、区分Ｓnm内に存在するすべての視線Ｌを加算対象として行われる。これにより、それぞれの高さ区間における度数が求められ、視線通過の頻度分布が算出される。
【００３１】
地形認識部２３は、ある区分Ｓnmに関する視線通過の頻度分布に基づいて、その区分Ｓnmにおける地面高ｙnmを特定する。地面高ｙnmの特定手法としては、例えば、以下の３つの手法が考えられる。
【００３２】
第１の手法は、高さの低い方を優先しつつ、個々の高さの度数（視線通過の出現頻度）の大小に基づいて、地面高ｙnmを決定する手法である。具体的には、視線通過の頻度分布において、所定の閾値Ｔh以上の度数を有する高さ区間のうち、最も低い高さ区間、または、その直下（１つ下）の高さ区間を、区分Ｓnmの地面高ｙnmとする。例えば、図７のケースでは、閾値Ｔh以上の高さ区間が７つ存在するが、その中で高さが最も低い区間（-0.2m〜-0.1m）、または、その直下の高さ区間（-0.3m〜-0.2m）が地面高ｙnmとなる。
【００３３】
移動体が自身の移動のために周囲地形を認識しようとする場合、極力広い範囲の地面を観測し、大局的に周囲の地形を捉えることが望ましい。本実施形態では、三次元空間上において、視線、すなわち、ステレオカメラ２と対象物とを結ぶ仮想的な直線の通過頻度を調べることによって地面高を特定する。この地面高の特定手法は、遠景が見えている空間には障害物は存在しないという知見に基づいており、地面自体の距離情報が希薄となる遠方を含む広い範囲で地形の認識を可能とする。ある区分Ｓnm内を多数の視線Ｌが通過している場合、その区分Ｓnm内には視線Ｌを遮る対象物が存在しないということになる。このような知得に基づいて、第１の手法では、高さ区間を下から上に向かって順次調べていき、最初に閾値Ｔh以上の度数を有する高さ区間、または、その直下の高さ区間が地面高ｙnmであるとみなされる。
【００３４】
第２の手法は、第１の手法と同様の知得に基づいたものであるが、高さの低い方から数えた累積度数の大小に基づいて、地面高ｙnmを決定する手法である。具体的には、視線通過の頻度分布において、高さの低い方から高い方に向かって、視線通過の度数を累積していくことにより、累積度数を算出する。そして、この累積度数が所定の閾値以上になる高さ区間、または、その直下の高さ区間が地面高ｙnmとして特定される。
【００３５】
第３の手法は、高さ方向に隣接した高さのうち、度数が高いものをグループ化することにより、地面高ｙnmを決定する手法である。具体的には、視線通過の頻度分布において、度数が所定の閾値以上となる高さ区間が高さ方向に連続している領域は、対象物が存在しない空間領域と判断される。また、度数が閾値未満となる高さ区間が高さ方向に連続している領域は、対象物が存在する非空間領域と判断される。すべての高さ区間は、空間領域または非空間領域のいずれかに分割される。このようにして特定された空間領域（複数の空間領域が特定されることもある）のうちで、高さ方向の範囲が最も広い空間領域が地面上の空間であると判断される。そして、地面上の空間に相当する空間領域における最も低い高さ区間を基準とし、その直下の高さ区間が地面高ｙnmとして特定される。例えば、図７のケースでは、閾値Ｔh（第１の手法の閾値Ｔhとは異なる）以上の７つの高さ区間（-0.2〜0.5）が地面上の空間と判断され、その中で最も低い高さ区間は（-0.2〜-0.1）となる。したがって、地面高ｙnmは、高さ区間（-0.2〜-0.1）の直下の高さ区間に相当する（-0.3〜-0.2）となる。第３の手法は、第１および第２の手法と比較して、地面に穴が存在する場合、或いは、ステレオマッチングにおけるミスマッチが多い場合等に有効である。
【００３６】
ある区分Ｓnmにおいて、上述した手法における地面特定の要件を満足しない場合、地形認識部２３は、その区分Ｓnmの地面高ｙnmを、周囲の地面高ｙnm'に基づいて推定する。例えば、周囲の地面高ｙnm'に基づき補間する内挿処理によって、地面高ｙnmが決定される。
【００３７】
地形認識部２３において算出された第１のフレームの地形データＧnは、後段の地形マップ生成部１４に出力される。この地形データＧnは、第１のフレームにおいてステレオカメラ２の視野範囲内に写し出された地面の三次元的形状を表すデータであり、具体的には、第１のフレームで設定された区分Ｓnmを算出単位とした地面高ｙnmの集合である。
【００３８】
このように、地形データ生成部１２は、高さ方向（Ｙ方向）を除く二方向（Ｘ方向およびＺ方向）で三次元空間を行列状に分割することにより、三次元空間上に複数の区分Ｓnmを設定する。それぞれの区分Ｓnmに関して、高さ方向に関する視線Ｌ群の通過頻度分布が生成される。そして、それぞれの区分Ｓnmにおいて、視線Ｌ群の統計的な通過経路を調べることにより、地面高ｙnmが決定される。これにより、不整地のように起伏や凹凸のある状況、或いは、遠方にあって地面自体の距離データが得られにくい箇所を視野内に含む状況であっても、地面高Ｙnmを特定することができる。その結果、地面高Ｙnmを広い範囲で安定的かつ精度よく算出することができるため、起伏のある地面の三次元形状を精度よく認識することが可能となる。
【００３９】
なお、それぞれの地面高ｙnmの算出にあたっては、対応する区分Ｓnm内の情報だけでなく、その周囲に存在する所定範囲内の区分Ｓnm'における度数の合計や平均等の統計量も考慮した上で決定してもよい。
【００４０】
また、地形認識部２３は、上述した視線通過頻度分布より特定される高さに、周囲の地面高ｙnm'の連続性を加味した上で、地面高ｙnmを特定してもよい。例えば、ある区分Ｓnmに関して特定された地面高ｙnmが、区分Ｓnmの周囲の地面高ｙnm'よりも所定の閾値以上異なる場合、周囲の地面高ｙnm'に基づいて、地面高ｙnmを補正（平準化）するといった如くである。これにより、地形の三次元的形状を一層精度よく認識することが可能となる。
【００４１】
一方、自己運動検出部１３は、第１および第２のフレームの画像データＰn，Ｐn-1と、第１および第２のフレームの距離データＤn，Ｄn-1とに基づいて、前後のフレーム間における移動体自身の移動量（換言すれば、ステレオカメラ２の移動量）を検出し、前後のフレーム間における移動体の移動状態（運動状態）を表す移動量データＭnを生成・出力する。移動体の移動状態は、３つの回転成分（ヨー、ロール、ピッチ）と３つの並進成分（Ｘ方向、Ｙ方向成分、Ｚ方向成分）とによって規定される。図８は、自己運動検出部１３のブロック構成図である。自己運動検出部１３は、基準領域設定部３０、対応領域設定部３１、三次元位置算出部３２および移動量検出部３３とで構成されている。
【００４２】
基準領域設定部３０は、まず、第２のフレームの画像データＰn-1によって規定される画像平面上に、ｉ個（例えば48個）の基準領域Ｒiを設定する。これらの基準領域Ｒiは、画像平面全体に極力分散するように設定され、それぞれの基準領域Ｒiは、8×8画素の面積を有している。基準領域Ｒiの設定に際して留意すべき点は、二次元マッチングを行うのに「適した領域」を設定すべき点である。「適した領域」とは、輝度エッジを有する領域、具体的には、領域内における互いに隣接した画素間の輝度変化が大きな領域である。したがって、第２のフレームの画像平面上に基準領域Ｒiを設定する場合、その前提として、二次元マッチングを行うのに「適した領域」であるか否かの評価が行われる。本実施形態では、一例として、水平方向における輝度差の総和と垂直方向における輝度差の総和との和が、所定の閾値以上であることを基準領域Ｒiの条件としている。
【００４３】
具体的には、基準領域Ｒiの候補となる領域において、水平方向に隣接した画素対毎に輝度変化量（絶対値）Δｐ1が算出される。基準領域Ｒiは8×8画素の面積を有しているので、64個の輝度変化量Δｐ1が算出される。その際、最右の画素列（または最左の画素列）については、領域外の隣接画素列を用いて、輝度変化量Δｐ1が算出される。これらの64個の輝度変化量Δｐ1の総和を水平輝度和ＰＡ1とする。つぎに、この候補領域に関して、垂直方向に隣接した画素対毎に輝度変化量（絶対値）Δｐ2が算出される。基準領域Ｒiは8×8画素の面積を有しているので、64個の輝度変化量Δｐ2が算出される。その際、最下の画素行（または最上の画素行）については、領域外の隣接画素行を用いて、輝度変化量Δｐ2が算出される。これらの64個の輝度変化量Δｐ2の総和を垂直輝度和ＰＡ2とする。そして、水平輝度総和ＰＡ1と垂直輝度和ＰＡ2との和を求め、この和（ＰＡ1＋ＰＡ2）が所定の閾値よりも大きい場合、この領域が基準領域Ｒiとして設定される。
【００４４】
このような候補領域の設定・評価を繰り返すことにより、第２のフレームの画像平面上に、ｉ個の基準領域Ｒiが設定される。ｉ個の基準領域Ｒiが設定されると、それぞれに関する画像平面上の位置が後段の三次元位置算出部３２に出力される。基準領域Ｒiの位置は基準点ａiによって特定され、一例として、基準領域Ｒiの左下の座標（ｉ，ｊ）を基準点ａiとする。
【００４５】
一方、対応領域設定部３１は、第１のフレームの画像データＰnによって規定される画像平面上に、それぞれの基準領域Ｒiと輝度的な相関を有する対応領域Ｃiを設定する。そのために、対応領域設定部３１は、基準領域Ｒiと、所定の探索範囲内に設定された8×8画素の画素ブロック（対応領域Ｃiの候補）との間におけるシティブロック距離ＣＢの二次元的な分布を求める。シティブロック距離ＣＢの分布は、探索範囲内に存在する画素ブロック毎に、上述した数式１の演算を行うことによって生成される。探索範囲の全域に渡って比較対象を水平／垂直方向に１画素ずつオフセットさせながら（二次元マッチング）、比較対象ごとに１つのシティブロック距離ＣＢが算出される。そして、この二次元的な分布を参照し、対応領域Ｃiの候補の中で、シティブロック距離ＣＢの値が極小となるものが対応領域Ｃiとして設定される。
【００４６】
このような相関性評価をそれぞれの基準領域Ｒiに対して行うことで、第１のフレームの画像平面上に、基準領域Ｒiの個数相当の対応領域Ｃiが特定される。これらの対応領域Ｃiが設定されると、それぞれに関する画像平面上の位置が後段の三次元位置算出部３２に出力される。対応領域Ｃiの位置は対応点ｂiによって特定され、一例として、対応領域Ｃiの左下の座標（ｉ，ｊ）を対応点ｂiとする。
【００４７】
なお、上述した輝度エッジの評価および二次元マッチングについては、本出願人の先願である特願平１１−２６１４３８号（特開２００１−８２９５５号公報）に詳述されているので必要ならば参照されたい。
【００４８】
また、本実施形態では、第２のフレームの画像平面上に基準領域Ｒiを設定し、第１のフレームの画像平面上に対応領域Ｃiを設定する例について説明したが、設定対象となるフレームを逆にしてもよい。この場合、第１のフレームの画像平面上に基準領域Ｒiを設定し、第２のフレームの画像平面上に対応領域Ｃiを設定することになる。
【００４９】
三次元位置算出部３２は、画像平面上に設定されたｉ個の基準点ａiのうち、適宜の手法で任意の３点を少なくとも選択し、選択された３点の位置的関係の評価を行った上で、それぞれの三次元空間上の位置を算出する。本実施形態では、選択された３つの基準点ａiが下記の２つの評価条件の双方を具備するか否かを評価する。
【００５０】
第１の評価条件は、画像平面上において、３つの基準点ａiによって規定される２つの線分の長さがそれぞれ所定の閾値以上であることである。後述するように、本実施形態では、少なくとも３つの基準点ａiによって規定される三角形と、３つの対応点ｂiによって規定される三角形との変化に基づいて、移動体の移動量が算出される。そのため、三角形を構成する辺の長さがあまり短いと、三次元空間において有効な三角形を設定できないため、算出される移動量の誤差が大きくなる。そこで、本評価条件によって、ある基準点ａiを基準に設定される２辺の長さを評価する。
【００５１】
第２の評価条件は、三次元空間上において、３つの基準点ａiによって規定される２つのベクトルのなす角度が45°以上で135°以下であることである。３つの基準点ａiが本条件を満たさない場合、これらの基準点ａiによって規定される形状は直線に近くなるため、三次元空間上に有効な三角形を設定することができない。そこで、移動量の算出精度を担保すべく、本評価条件によって、三角形の形状を評価する。
【００５２】
三次元位置算出部３２は、上記２条件を具備する基準点ａiのそれぞれに関して、その画像平面上の位置（ｉ，ｊ）と、これに対応する視差ｄとのセットに基づいて、三次元空間上の位置（Ｘ，Ｙ，Ｚ）を算出する。ここで、視差ｄは、第２のフレームの距離データＤn-1のうち、例えば、基準領域Ｒi内に含まれる視差群の平均視差を用いることができる。また、（ｉ，ｊ，ｄ）のセットから三次元位置（Ｘ，Ｙ，Ｚ）への座標変換は、上述した数式２に基づいて行われる。これにより、３つの基準点ａiの三次元空間上の位置（Ｘ，Ｙ，Ｚ）がそれぞれ算出される。三次元空間上の３点が特定されると、三次元空間上の三角形が一義的に特定される。したがって、第２のフレームにおける三角形とステレオカメラ２との間の位置関係および姿勢関係が特定される。
【００５３】
また、三次元位置算出部３２は、上記２条件を具備する基準点ａiに対応する対応点ｂiのそれぞれに関して、その画像平面上の位置（ｉ，ｊ）と、これに対応する視差ｄとのセットに基づいて、数式２に従い、三次元空間上の位置（Ｘ，Ｙ，Ｚ）を算出する。ここで、視差ｄは、第１のフレームの距離データＤnのうち、例えば、対応領域Ｃi内に含まれる視差群の平均視差を用いることができる。これにより、３つの対応点ａiの三次元位置（Ｘ，Ｙ，Ｚ）がそれぞれ算出され、第１のフレームにおける三角形とステレオカメラ２との間の位置関係および姿勢関係が特定される。
【００５４】
移動量検出部３３は、３つの基準点ａiの三次元位置（Ｘ，Ｙ，Ｚ）と、３つの対応領域Ｃiの三次元位置（Ｘ，Ｙ，Ｚ）とに基づいて、前後のフレーム間における移動体の移動量を検出する。周知のように、三次元空間上の３点が特定されると、実空間上の三角形が一義的に特定されるため、移動体の自由度を規定する６つの成分が算出可能となる。６つの成分とは、３つの回転成分（ヨー、ロール、ピッチ）と３つの並進成分（Ｘ方向成分、Ｙ方向成分、Ｚ方向成分）である。これらの成分は、周知の演算手法を用いて算出され、移動体の移動量を示す移動量データＭnとして、後段の地形マップ生成部１４に出力される。
【００５５】
なお、三次元位置算出部３２は、移動体の移動量の検出精度を高めるために、同一フレームにおいて、３つの基準点ａiのセットの設定・三次元位置の算出を所定回数分繰り返し、必要サンプル数だけ算出することが好ましい。この場合、移動量検出部３３は、サンプル数毎に算出された６つの成分のそれぞれに関するヒストグラムを生成し、出現度数の最も高い成分を特定することにより、移動量データＭnを生成することが好ましい。
【００５６】
地形マップ生成部１４は、地形データ生成部１２からの地形データＧnと、自己運動検出部１３からの移動量データＭnとに基づいて、地形の三次元的形状を示すマップデータを更新する。具体的には、既に作成されている地形マップに対して、移動体の移動量を考慮した上で、フレーム毎に認識された地形が順次追加される。例えば、ステレオカメラ２を基準点とする場合、既に作成されている地形マップを移動体の移動量だけオフセットさせた上で、新たに認識された地形がマップに合成される。また、例えば、地形の計測開始位置を基準とする場合、新たに認識された地形を移動体の移動量だけオフセットさせた上で、それが地形マップに合成される。以上の処理を通じて、地形のマップデータは、地形マップ生成部１４によって随時更新され、更新されたマップデータがマップデータメモリ１１に格納される。
【００５７】
このように、本実施形態によれば、ステレオ画像に基づいて地形が認識され、ステレオ画像に基づいて移動体の移動量が検出される。そして、移動体の移動量を考慮した上で、地形の三次元的形状を示すマップに新たに認識された地形が随時追加されていく。その結果、移動体の移動に伴い、広範囲な地形マップを精度よく生成することができる。
【００５８】
また、移動体が通過した経路上の地形形状は地形マップとしてマップデータメモリ１１に記憶されるため、このデータを利用した移動制御を行えば、より高度な移動体の移動制御を行うことが可能となる。例えば、移動体が前方を向いたままバックするといった如くである。
【００５９】
また、本実施形態によれば、移動体の移動量をステレオ画像のみから算出することができる。したがって、移動体の姿勢や速度等を検出するセンサを別途追加する必要がなくなるため、センサ数の削減を図ることができる。
【００６０】
（第２の実施形態）
本実施形態の特徴は、図１に示した地形認識装置１の一部を構成する地形データ生成部１２を改良した点にある。具体的には、図３に示した地形データ生成部１２の構成に対象物ヒストグラム生成部２４を追加し、対象物の存在頻度分布を考慮した上で、地面高ｙnmを特定する点にある。図９は、第２の実施形態にかかる地形データ生成部１２のブロック構成図である。同図において、図３に示した構成ブロックと同一のものについては、同一の番号を付して、ここでの説明を省略する。
【００６１】
対象物ヒストグラム生成部２４は、三次元位置算出部２０によって算出された三次元位置（Ｘ，Ｙ，Ｚ）に基づいて、高さ方向における対象物の存在頻度分布を生成する。この頻度分布の生成は、視線通過の頻度分布と同様に、図５に示した区分Ｓnm毎に行われる。図１０は、ある区分Ｓnmに関する対象物の存在頻度ヒストグラムの一例を示す図である。対象物の存在頻度分布は、図７に示した視線通過頻度ヒストグラムと同様に、高さｙを縦軸として、予め設定された高さ区間（例えば、0.1ｍ）毎に縦軸が区切られている。そして、ある対象物の三次元位置（Ｘ，Ｙ，Ｚ）が区分Ｓnm内に存在する場合、その対象物が存在する高さ区間の度数に１を加算する。この加算処理は、区分Ｓnm内に存在するすべての対象物を加算対象として行われる。これにより、それぞれの高さ区間における対象物の存在度数が算出される。
【００６２】
地形認識部２３は、視線通過の頻度分布と対象物の存在頻度分布とに基づいて、それぞれの区分Ｓnmにおける地面高ｙnmを特定する。具体的には、まず、第１の実施形態で説明した手法に従い、区分Ｓnmに関する視線通過の頻度分布より、視線ベースの地面高ｙ1nmが算出される。つぎに、区分Ｓnmに関する対象物の存在頻度分布より、対象物ベースの地面高ｙ2nmが算出される。この地面高ｙ2nmは、所定の閾値Ｔh以上の度数を有する高さ区間のうち、最も低い高さ区間として特定することができる。例えば、図１０のケースでは、閾値Ｔh以上の高さ区間が７つ存在するが、その中で高さが最も低い区間（-0.3m〜-0.2m）が地面高ｙ2nmとなる。そして、視線ベースの地面高ｙ1nmと対象物ベースの地面高ｙ2nmとに基づいて、最終的な地面高ｙnmが算出される。最終的な地面高Ｙnmは、例えば、視線ベースの地面高Ｙ1nと対象物ベースの地面高Ｙ2nmとの単純平均または加重平均より算出することができる。この場合の加重平均の重みとして、例えば、対象物ベースの地面高Ｙ2nmの信頼度を用いてもよい。すなわち、対象物ベースの地面高Ｙ2nmの信頼度が大であれば、Ｙ2nmの重みを増やし、これが小であれば、Ｙ2nmの重みを増やすといった如くである。信頼度としては、例えば、区分Ｓnmに関する対象物の存在頻度のうち、閾値Ｔh以上である高さ区間における頻度の合計値と、所定の判定基準閾値との比を用いる方法がある。
【００６３】
このように、本実施形態によれば、第１の実施形態と同様の効果を有する。特に、本実施形態では、視線通過の頻度分布のみならず、対象物の存在頻度分布をも考慮して、地面高ｙnmを算出しているため、地面の三次元的形状を一層精度よく認識することが可能となる。
【００６４】
なお、対象物の存在頻度ヒストグラムにおいて、特定された地面高以下の頻度を除けば、地面上に存在する障害物のみを認識することも可能である。
【００６５】
【発明の効果】
このように、本発明によれば、移動体に搭載されたステレオカメラより得られるステレオ画像を処理することにより、地面の三次元的形状を精度よく認識でき、かつ、移動体の移動に伴い、地形認識を広範囲に亘って行うことが可能となる。また、ステレオ画像の処理を通じて、移動体の移動量を精度よく検出することができるため、移動体の運動状態を検出するセンサ数の削減を図ることが可能となる。
【図面の簡単な説明】
【図１】第１の実施形態にかかる地形認識装置のブロック構成図
【図２】基準画像に設定される画素ブロックＰＢijの説明図
【図３】地形データ生成部のブロック構成図
【図４】三次元空間上に設定される視線Ｌの説明図
【図５】三次元空間上に設定される区分Ｓnmの説明図
【図６】区分Ｓnmを通過する視線Ｌ群の一例を示す図
【図７】区分Ｓnmに関する視線通過頻度ヒストグラムの一例を示す図
【図８】自己運動検出部のブロック構成図
【図９】第２の実施形態にかかる地形データ生成部のブロック構成図
【図１０】区分Ｓnmに関する対象物の存在頻度ヒストグラムの一例を示す図
【符号の説明】
１ 地形認識装置
２ ステレオカメラ
２ａ メインカメラ
２ｂ サブカメラ
４ Ａ／Ｄコンバータ
５ Ａ／Ｄコンバータ
６ 画像補正部
７ ステレオ画像処理部
８ 距離データメモリ
９ 画像データメモリ
１０ マイクロコンピュータ
１１ マップデータメモリ
１２ 地形データ生成部
１３ 自己運動検出部
１４ 地形マップ生成部
２０ 三次元位置算出部
２１ 視線設定部
２２ 視線ヒストグラム生成部
２３ 地形認識部
２４ 対象物ヒストグラム生成部
３０ 基準領域設定部
３１ 対応領域設定部
３２ 三次元位置算出部
３３ 移動量検出部

Claims (6)

1. 移動体に搭載され、地面の三次元的形状を認識する地形認識装置において、
   地面を含む景色を時系列的に撮像し、一対の画像データをフレーム毎に出力するステレオカメラと、
   前記一対の画像データに基づき、ステレオマッチングによって視差を算出するとともに、当該算出された視差と、前記画像データにより規定される画像平面上の位置とが対応付けられた距離データをフレーム毎に出力するステレオ画像処理部と、
   第１のフレームの距離データに基づいて、前記第１のフレームにおける地形の三次元的形状を示す地形データを生成する地形データ生成部と、
   前記第１のフレームの画像データと、前記第１のフレームよりも前の第２のフレームの画像データと、前記第１のフレームの距離データと、前記第２のフレームの距離データとに基づいて、前記第１のフレームと前記第２のフレームとの間における前記移動体の移動量を検出する自己運動検出部と、
   前記自己運動検出部によって検出された前記移動量を考慮した上で、前記地形データ生成部によって生成された前記地形データを順次合成することにより、地形マップを生成する地形マップ生成部とを有し、
   前記地形データ生成部は、
   前記第１の距離データに基づいて、それぞれの対象物の三次元位置を算出する三次元位置算出部と、
   三次元空間上において、前記対象物のそれぞれの三次元位置と前記ステレオカメラの取付位置とを結ぶ視線群を設定する視線設定部と、
   高さ方向を除く二方向で、三次元空間を行列状に分割することによって、三次元空間上に複数の区分を設定するとともに、前記視線設定部によって設定された視線群に基づいて、前記区分毎に、高さ方向における視線通過の頻度分布を生成する視線ヒストグラム生成部と、
   前記視線通過の頻度分布に基づいて、前記区分のそれぞれにおける地面の高さを特定する地形認識部とを有することを特徴とする地形認識装置。
2. 前記地形データ生成部は、
   前記対象物のそれぞれの三次元位置に基づいて、前記区分毎に、高さ方向における対象物の存在頻度分布を生成する対象物ヒストグラム生成部をさらに有し、
   前記地形認識部は、前記視線通過の頻度分布と前記対象物の存在頻度分布とに基づいて、前記区分のそれぞれにおける地面の高さを特定することを特徴とする請求項１に記載された地形認識装置。
3. 前記地形認識部は、ある区分に関して特定された地面の高さが、前記区分の周囲の地面の高さよりも所定の閾値以上異なる場合、前記区分の地面の高さを、前記区分の周囲の地面の高さに基づいて、補正することを特徴とする請求項１または２に記載された地形認識装置。
4. 前記自己運動量検出部は、
   前記第１のフレームまたは前記第２のフレームの一方の画像データによって規定される画像平面上に複数の基準領域を設定し、前記画像平面上における前記基準領域の位置を示す基準点を特定する基準領域設定部と、
   前記第１のフレームまたは前記第２のフレームの他方の画像データによって規定される画像平面上に、前記基準領域のそれぞれと相関を有する対応領域を設定し、前記画像平面上における前記対応領域の位置を示す対応点を特定する対応領域設定部と、
   前記基準領域設定部によって特定された複数の基準点のうち、３つの基準点を選択し、当該選択された基準点のそれぞれの三次元位置を、前記一方のフレームの距離データに基づいて算出するとともに、前記対応領域設定部によって特定された複数の対応点のうち、３つの対応点を選択し、当該選択された対応点のそれぞれの三次元位置を、前記他方のフレームの距離データに基づいて算出する三次元位置算出部と、
   前記三次元位置算出部によって算出された前記基準点の三次元位置と前記対応点の三次元位置とに基づいて、前記第１のフレームと前記第２のフレームとの間における移動体の移動量を検出する前記移動量検出部と
   を有することを特徴とする請求項１から３のいずれかに記載された地形認識装置。
5. 前記三次元位置算出部は、前記選択された基準点の位置的関係の評価を行った上で、前記選択された基準点の三次元位置を算出することを特徴とする請求項４に記載された地形認識装置。
6. 移動体に搭載され、地面の三次元的形状を認識する地形認識方法において、
   ステレオカメラによって、地面を含む景色を時系列的に撮像し、一対の画像データをフレーム毎に出力するステップと、
   前記一対の画像データに基づき、ステレオマッチングによって視差を算出するとともに、当該算出された視差と、前記画像データにより規定される画像平面上の位置とが対応付けられた距離データをフレーム毎に出力するステップと、
   第１のフレームの距離データに基づいて、前記第１のフレームにおける地形の三次元的形状を示す地形データを生成するステップと、
   前記第１のフレームの画像データと、前記第１のフレームよりも前の第２のフレームの画像データと、前記第１のフレームの距離データと、前記第２のフレームの距離データとに基づいて、前記第１のフレームと前記第２のフレームとの間における前記移動体の移動量を検出するステップと、
   前記検出された移動量を考慮した上で、フレーム毎に生成された前記地形データを順次合成することにより、地形マップを生成するステップとを有し、
   前記地形データを生成するステップは、
   前記第１の距離データに基づいて、それぞれの対象物の三次元位置を算出するステップと、
   三次元空間上において、前記対象物のそれぞれの三次元位置と前記ステレオカメラの取付位置とを結ぶ視線群を設定するステップと、
   高さ方向を除く二方向で、三次元空間を行列状に分割することによって、三次元空間上に複数の区分を設定するとともに、前記設定された視線群に基づいて、前記区分毎に、高さ方向における視線通過の頻度分布を生成するステップと、
   前記視線通過の頻度分布に基づいて、前記区分のそれぞれにおける地面の高さを特定するステップとを有することを特徴とする地形認識装置。

Images