JP2004144644A - Topography recognition device, topography recognition means, and moving quantity detection method of moving body - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To accurately recognize the three-dimensional shape of the ground surface by using a stereo camera loaded on a moving body, and to perform the topography recognition over a wide range following movement of the moving body. <P>SOLUTION: A stereo image processing part 7 outputs distance data relative to each frame based on a pair of image data outputted in time series from the stereo camera 2. A topographic data generation part 12 generates the topographic data in the first frame based on the distance data in the first frame. A self-motion detection part 13 detects the moving quantity of the moving body between the first and second frames based on image data in the first frame, image data in the second frame before the first frame, and the distance data relative to the image data. A topographic map generation part 14 generates a topographic map by synthesizing sequentially the topographic data generated by the topographic data generation part 12 in consideration of the moving quantity detected by the self-motion detection part 13. <P>COPYRIGHT: (C)2004,JPO

Description

【００２７】
視線設定部２１は、三次元位置算出部２０によって算出された三次元位置（Ｘ，Ｙ，Ｚ）に基づいて、三次元空間におけるステレオカメラ２の取付位置と対象物の位置とを結ぶ視線Ｌを設定する。図４は、Ｘ軸、Ｙ軸およびＺ軸によって規定される三次元空間において設定される視線Ｌの説明図である。ある対象物Ｂ（地面および障害物の双方を含む）に関する視線Ｌは、その対象物Ｂの三次元位置（Ｘ，Ｙ，Ｚ）とステレオカメラ２の取付位置Ｃ（０，ｙ１，０）とを通る直線式で規定され、この式によって、対象物Ｂまでの視線Ｌの通過経路が一義的に特定される。ここで、ｙ１は、ステレオカメラ２直下の地面を基準としたステレオカメラ２の取付高さである。視線設定部２１は、すべての対象物Ｂを演算対象として、それぞれの対象物Ｂに関する視線Ｌを順次算出する。１回の処理サイクルにおいて算出される視線Ｌ群の個数は、対象物Ｂの個数相当、換言すれば、１フレームの距離データを構成する視差ｄ群の個数相当である。
【００２８】
視線ヒストグラム生成部２２は、地面高の算出単位となる区分Ｓｎｍを三次元空間上に設定する。図５は三次元空間上に設定される区分Ｓｎｍの説明図である。三次元空間は、高さ方向を除く二方向で行列状、すなわち、Ｘ方向にＮ個、Ｚ方向にＭ個にそれぞれ分割される。これにより、三次元空間にＮ×Ｍ個の四角柱状の区分Ｓｎｍが定義される。地形マップの分解能は、１つの区分Ｓｎｍの底面積（横断面積）の大きさに依存しており、これが小さいほど地形マップの分解能（上面視の分解能）が向上する。ただし、区分Ｓｎｍの底面積が小さすぎると、地面高の算出精度が低下してしまうおそれがある。なぜなら、視線Ｌ群の統計処理よって地面高を決定する関係上、区分Ｓｎｍの底面積が小さくなると、１つの区分Ｓｎｍ内を通過する視線Ｌの個数（サンプル数）が減少してしまうからである。したがって、区分Ｓｎｍの底面積（換言すれば、三次元空間の分割数）は、地形マップに要求される分解能と、地面高の算出精度との双方を考慮した上で、決定する必要がある。本実施形態では、１個当たりの区分Ｓｎｍの底形状を、一例として、２０ｃｍ四方に設定している。このようにして、三次元空間を高さ方向を除く二方向で行列状に分割することによって、三次元空間上に複数の立体的な区分Ｓｎｍが格子状に設定される。１つの区分Ｓｎｍ内には、例えば、図６に示すような通過経路で視線Ｌが存在する。ある区分Ｓｎｍ内を多数の視線Ｌが通過するということは、その区分Ｓｎｍ内に視線を遮る物体（障害物）が存在しないことを意味する。
【００２９】
なお、三次元空間を格子状に分割することにより四角柱状の区分Ｓｎｍを設定するのは一例であって、区分Ｓｎｍの設定手法や立体形状は、これに限定されるものではない。例えば、区分Ｓｎｍを六角柱状にしてもよい。
【００３０】
視線ヒストグラム生成部２２は、視線設定部２１によって設定された視線Ｌ群に基づいて、高さ方向における視線通過の頻度分布を生成する。この頻度分布の生成は、区分Ｓｎｍ毎に行われる。図７は、ある区分Ｓｎｍに関する視線通過頻度ヒストグラムの一例を示す図である。同図に示すように、視線通過の頻度分布は、例えば、高さｙを縦軸として、予め設定された高さ区間（例えば、０．１ｍ）毎に縦軸が区切られている。地形マップの高さ方向の分解能は、１区間当たりの高さ区間の幅に依存している。そして、１つの視線Ｌがある高さ区間を通過する毎、その区間の度数に１を加算する。このような視線Ｌの通過回数の加算は、区分Ｓｎｍ内に存在するすべての視線Ｌを加算対象として行われる。これにより、それぞれの高さ区間における度数が求められ、視線通過の頻度分布が算出される。
【００３１】
地形認識部２３は、ある区分Ｓｎｍに関する視線通過の頻度分布に基づいて、その区分Ｓｎｍにおける地面高ｙｎｍを特定する。地面高ｙｎｍの特定手法としては、例えば、以下の３つの手法が考えられる。
【００３２】
第１の手法は、高さの低い方を優先しつつ、個々の高さの度数（視線通過の出現頻度）の大小に基づいて、地面高ｙｎｍを決定する手法である。具体的には、視線通過の頻度分布において、所定の閾値Ｔｈ以上の度数を有する高さ区間のうち、最も低い高さ区間、または、その直下（１つ下）の高さ区間を、区分Ｓｎｍの地面高ｙｎｍとする。例えば、図７のケースでは、閾値Ｔｈ以上の高さ区間が７つ存在するが、その中で高さが最も低い区間（−０．２ｍ〜−０．１ｍ）、または、その直下の高さ区間（−０．３ｍ〜−０．２ｍ）が地面高ｙｎｍとなる。
【００３３】
移動体が自身の移動のために周囲地形を認識しようとする場合、極力広い範囲の地面を観測し、大局的に周囲の地形を捉えることが望ましい。本実施形態では、三次元空間上において、視線、すなわち、ステレオカメラ２と対象物とを結ぶ仮想的な直線の通過頻度を調べることによって地面高を特定する。この地面高の特定手法は、遠景が見えている空間には障害物は存在しないという知見に基づいており、地面自体の距離情報が希薄となる遠方を含む広い範囲で地形の認識を可能とする。ある区分Ｓｎｍ内を多数の視線Ｌが通過している場合、その区分Ｓｎｍ内には視線Ｌを遮る対象物が存在しないということになる。このような知得に基づいて、第１の手法では、高さ区間を下から上に向かって順次調べていき、最初に閾値Ｔｈ以上の度数を有する高さ区間、または、その直下の高さ区間が地面高ｙｎｍであるとみなされる。
【００３４】
第２の手法は、第１の手法と同様の知得に基づいたものであるが、高さの低い方から数えた累積度数の大小に基づいて、地面高ｙｎｍを決定する手法である。具体的には、視線通過の頻度分布において、高さの低い方から高い方に向かって、視線通過の度数を累積していくことにより、累積度数を算出する。そして、この累積度数が所定の閾値以上になる高さ区間、または、その直下の高さ区間が地面高ｙｎｍとして特定される。
【００３５】
第３の手法は、高さ方向に隣接した高さのうち、度数が高いものをグループ化することにより、地面高ｙｎｍを決定する手法である。具体的には、視線通過の頻度分布において、度数が所定の閾値以上となる高さ区間が高さ方向に連続している領域は、対象物が存在しない空間領域と判断される。また、度数が閾値未満となる高さ区間が高さ方向に連続している領域は、対象物が存在する非空間領域と判断される。すべての高さ区間は、空間領域または非空間領域のいずれかに分割される。このようにして特定された空間領域（複数の空間領域が特定されることもある）のうちで、高さ方向の範囲が最も広い空間領域が地面上の空間であると判断される。そして、地面上の空間に相当する空間領域における最も低い高さ区間を基準とし、その直下の高さ区間が地面高ｙｎｍとして特定される。例えば、図７のケースでは、閾値Ｔｈ（第１の手法の閾値Ｔｈとは異なる）以上の７つの高さ区間（−０．２〜０．５）が地面上の空間と判断され、その中で最も低い高さ区間は（−０．２〜−０．１）となる。したがって、地面高ｙｎｍは、高さ区間（−０．２〜−０．１）の直下の高さ区間に相当する（−０．３〜−０．２）となる。第３の手法は、第１および第２の手法と比較して、地面に穴が存在する場合、或いは、ステレオマッチングにおけるミスマッチが多い場合等に有効である。
【００３６】
ある区分Ｓｎｍにおいて、上述した手法における地面特定の要件を満足しない場合、地形認識部２３は、その区分Ｓｎｍの地面高ｙｎｍを、周囲の地面高ｙｎｍ’に基づいて推定する。例えば、周囲の地面高ｙｎｍ’に基づき補間する内挿処理によって、地面高ｙｎｍが決定される。
【００３７】
地形認識部２３において算出された第１のフレームの地形データＧｎは、後段の地形マップ生成部１４に出力される。この地形データＧｎは、第１のフレームにおいてステレオカメラ２の視野範囲内に写し出された地面の三次元的形状を表すデータであり、具体的には、第１のフレームで設定された区分Ｓｎｍを算出単位とした地面高ｙｎｍの集合である。
【００３８】
このように、地形データ生成部１２は、高さ方向（Ｙ方向）を除く二方向（Ｘ方向およびＺ方向）で三次元空間を行列状に分割することにより、三次元空間上に複数の区分Ｓｎｍを設定する。それぞれの区分Ｓｎｍに関して、高さ方向に関する視線Ｌ群の通過頻度分布が生成される。そして、それぞれの区分Ｓｎｍにおいて、視線Ｌ群の統計的な通過経路を調べることにより、地面高ｙｎｍが決定される。これにより、不整地のように起伏や凹凸のある状況、或いは、遠方にあって地面自体の距離データが得られにくい箇所を視野内に含む状況であっても、地面高Ｙｎｍを特定することができる。その結果、地面高Ｙｎｍを広い範囲で安定的かつ精度よく算出することができるため、起伏のある地面の三次元形状を精度よく認識することが可能となる。
【００３９】
なお、それぞれの地面高ｙｎｍの算出にあたっては、対応する区分Ｓｎｍ内の情報だけでなく、その周囲に存在する所定範囲内の区分Ｓｎｍ’における度数の合計や平均等の統計量も考慮した上で決定してもよい。
【００４０】
また、地形認識部２３は、上述した視線通過頻度分布より特定される高さに、周囲の地面高ｙｎｍ’の連続性を加味した上で、地面高ｙｎｍを特定してもよい。例えば、ある区分Ｓｎｍに関して特定された地面高ｙｎｍが、区分Ｓｎｍの周囲の地面高ｙｎｍ’よりも所定の閾値以上異なる場合、周囲の地面高ｙｎｍ’に基づいて、地面高ｙｎｍを補正（平準化）するといった如くである。これにより、地形の三次元的形状を一層精度よく認識することが可能となる。
【００４１】
一方、自己運動検出部１３は、第１および第２のフレームの画像データＰｎ，Ｐｎ−１と、第１および第２のフレームの距離データＤｎ，Ｄｎ−１とに基づいて、前後のフレーム間における移動体自身の移動量（換言すれば、ステレオカメラ２の移動量）を検出し、前後のフレーム間における移動体の移動状態（運動状態）を表す移動量データＭｎを生成・出力する。移動体の移動状態は、３つの回転成分（ヨー、ロール、ピッチ）と３つの並進成分（Ｘ方向、Ｙ方向成分、Ｚ方向成分）とによって規定される。図８は、自己運動検出部１３のブロック構成図である。自己運動検出部１３は、基準領域設定部３０、対応領域設定部３１、三次元位置算出部３２および移動量検出部３３とで構成されている。
【００４２】
基準領域設定部３０は、まず、第２のフレームの画像データＰｎ−１によって規定される画像平面上に、ｉ個（例えば４８個）の基準領域Ｒｉを設定する。これらの基準領域Ｒｉは、画像平面全体に極力分散するように設定され、それぞれの基準領域Ｒｉは、８×８画素の面積を有している。基準領域Ｒｉの設定に際して留意すべき点は、二次元マッチングを行うのに「適した領域」を設定すべき点である。「適した領域」とは、輝度エッジを有する領域、具体的には、領域内における互いに隣接した画素間の輝度変化が大きな領域である。したがって、第２のフレームの画像平面上に基準領域Ｒｉを設定する場合、その前提として、二次元マッチングを行うのに「適した領域」であるか否かの評価が行われる。本実施形態では、一例として、水平方向における輝度差の総和と垂直方向における輝度差の総和との和が、所定の閾値以上であることを基準領域Ｒｉの条件としている。
【００４３】
具体的には、基準領域Ｒｉの候補となる領域において、水平方向に隣接した画素対毎に輝度変化量（絶対値）Δｐ１が算出される。基準領域Ｒｉは８×８画素の面積を有しているので、６４個の輝度変化量Δｐ１が算出される。その際、最右の画素列（または最左の画素列）については、領域外の隣接画素列を用いて、輝度変化量Δｐ１が算出される。これらの６４個の輝度変化量Δｐ１の総和を水平輝度和ＰＡ１とする。つぎに、この候補領域に関して、垂直方向に隣接した画素対毎に輝度変化量（絶対値）Δｐ２が算出される。基準領域Ｒｉは８×８画素の面積を有しているので、６４個の輝度変化量Δｐ２が算出される。その際、最下の画素行（または最上の画素行）については、領域外の隣接画素行を用いて、輝度変化量Δｐ２が算出される。これらの６４個の輝度変化量Δｐ２の総和を垂直輝度和ＰＡ２とする。そして、水平輝度総和ＰＡ１と垂直輝度和ＰＡ２との和を求め、この和（ＰＡ１＋ＰＡ２）が所定の閾値よりも大きい場合、この領域が基準領域Ｒｉとして設定される。
【００４４】
このような候補領域の設定・評価を繰り返すことにより、第２のフレームの画像平面上に、ｉ個の基準領域Ｒｉが設定される。ｉ個の基準領域Ｒｉが設定されると、それぞれに関する画像平面上の位置が後段の三次元位置算出部３２に出力される。基準領域Ｒｉの位置は基準点ａｉによって特定され、一例として、基準領域Ｒｉの左下の座標（ｉ，ｊ）を基準点ａｉとする。
【００４５】
一方、対応領域設定部３１は、第１のフレームの画像データＰｎによって規定される画像平面上に、それぞれの基準領域Ｒｉと輝度的な相関を有する対応領域Ｃｉを設定する。そのために、対応領域設定部３１は、基準領域Ｒｉと、所定の探索範囲内に設定された８×８画素の画素ブロック（対応領域Ｃｉの候補）との間におけるシティブロック距離ＣＢの二次元的な分布を求める。シティブロック距離ＣＢの分布は、探索範囲内に存在する画素ブロック毎に、上述した数式１の演算を行うことによって生成される。探索範囲の全域に渡って比較対象を水平／垂直方向に１画素ずつオフセットさせながら（二次元マッチング）、比較対象ごとに１つのシティブロック距離ＣＢが算出される。そして、この二次元的な分布を参照し、対応領域Ｃｉの候補の中で、シティブロック距離ＣＢの値が極小となるものが対応領域Ｃｉとして設定される。
【００４６】
このような相関性評価をそれぞれの基準領域Ｒｉに対して行うことで、第１のフレームの画像平面上に、基準領域Ｒｉの個数相当の対応領域Ｃｉが特定される。これらの対応領域Ｃｉが設定されると、それぞれに関する画像平面上の位置が後段の三次元位置算出部３２に出力される。対応領域Ｃｉの位置は対応点ｂｉによって特定され、一例として、対応領域Ｃｉの左下の座標（ｉ，ｊ）を対応点ｂｉとする。
【００４７】
なお、上述した輝度エッジの評価および二次元マッチングについては、本出願人の先願である特願平１１−２６１４３８号（特開２００１−８２９５５号公報）に詳述されているので必要ならば参照されたい。
【００４８】
また、本実施形態では、第２のフレームの画像平面上に基準領域Ｒｉを設定し、第１のフレームの画像平面上に対応領域Ｃｉを設定する例について説明したが、設定対象となるフレームを逆にしてもよい。この場合、第１のフレームの画像平面上に基準領域Ｒｉを設定し、第２のフレームの画像平面上に対応領域Ｃｉを設定することになる。
【００４９】
三次元位置算出部３２は、画像平面上に設定されたｉ個の基準点ａｉのうち、適宜の手法で任意の３点を少なくとも選択し、選択された３点の位置的関係の評価を行った上で、それぞれの三次元空間上の位置を算出する。本実施形態では、選択された３つの基準点ａｉが下記の２つの評価条件の双方を具備するか否かを評価する。
【００５０】
第１の評価条件は、画像平面上において、３つの基準点ａｉによって規定される２つの線分の長さがそれぞれ所定の閾値以上であることである。後述するように、本実施形態では、少なくとも３つの基準点ａｉによって規定される三角形と、３つの対応点ｂｉによって規定される三角形との変化に基づいて、移動体の移動量が算出される。そのため、三角形を構成する辺の長さがあまり短いと、三次元空間において有効な三角形を設定できないため、算出される移動量の誤差が大きくなる。そこで、本評価条件によって、ある基準点ａｉを基準に設定される２辺の長さを評価する。
【００５１】
第２の評価条件は、三次元空間上において、３つの基準点ａｉによって規定される２つのベクトルのなす角度が４５°以上で１３５°以下であることである。３つの基準点ａｉが本条件を満たさない場合、これらの基準点ａｉによって規定される形状は直線に近くなるため、三次元空間上に有効な三角形を設定することができない。そこで、移動量の算出精度を担保すべく、本評価条件によって、三角形の形状を評価する。
【００５２】
三次元位置算出部３２は、上記２条件を具備する基準点ａｉのそれぞれに関して、その画像平面上の位置（ｉ，ｊ）と、これに対応する視差ｄとのセットに基づいて、三次元空間上の位置（Ｘ，Ｙ，Ｚ）を算出する。ここで、視差ｄは、第２のフレームの距離データＤｎ−１のうち、例えば、基準領域Ｒｉ内に含まれる視差群の平均視差を用いることができる。また、（ｉ，ｊ，ｄ）のセットから三次元位置（Ｘ，Ｙ，Ｚ）への座標変換は、上述した数式２に基づいて行われる。これにより、３つの基準点ａｉの三次元空間上の位置（Ｘ，Ｙ，Ｚ）がそれぞれ算出される。三次元空間上の３点が特定されると、三次元空間上の三角形が一義的に特定される。したがって、第２のフレームにおける三角形とステレオカメラ２との間の位置関係および姿勢関係が特定される。
【００５３】
また、三次元位置算出部３２は、上記２条件を具備する基準点ａｉに対応する対応点ｂｉのそれぞれに関して、その画像平面上の位置（ｉ，ｊ）と、これに対応する視差ｄとのセットに基づいて、数式２に従い、三次元空間上の位置（Ｘ，Ｙ，Ｚ）を算出する。ここで、視差ｄは、第１のフレームの距離データＤｎのうち、例えば、対応領域Ｃｉ内に含まれる視差群の平均視差を用いることができる。これにより、３つの対応点ａｉの三次元位置（Ｘ，Ｙ，Ｚ）がそれぞれ算出され、第１のフレームにおける三角形とステレオカメラ２との間の位置関係および姿勢関係が特定される。
【００５４】
移動量検出部３３は、３つの基準点ａｉの三次元位置（Ｘ，Ｙ，Ｚ）と、３つの対応領域Ｃｉの三次元位置（Ｘ，Ｙ，Ｚ）とに基づいて、前後のフレーム間における移動体の移動量を検出する。周知のように、三次元空間上の３点が特定されると、実空間上の三角形が一義的に特定されるため、移動体の自由度を規定する６つの成分が算出可能となる。６つの成分とは、３つの回転成分（ヨー、ロール、ピッチ）と３つの並進成分（Ｘ方向成分、Ｙ方向成分、Ｚ方向成分）である。これらの成分は、周知の演算手法を用いて算出され、移動体の移動量を示す移動量データＭｎとして、後段の地形マップ生成部１４に出力される。
【００５５】
なお、三次元位置算出部３２は、移動体の移動量の検出精度を高めるために、同一フレームにおいて、３つの基準点ａｉのセットの設定・三次元位置の算出を所定回数分繰り返し、必要サンプル数だけ算出することが好ましい。この場合、移動量検出部３３は、サンプル数毎に算出された６つの成分のそれぞれに関するヒストグラムを生成し、出現度数の最も高い成分を特定することにより、移動量データＭｎを生成することが好ましい。
【００５６】
地形マップ生成部１４は、地形データ生成部１２からの地形データＧｎと、自己運動検出部１３からの移動量データＭｎとに基づいて、地形の三次元的形状を示すマップデータを更新する。具体的には、既に作成されている地形マップに対して、移動体の移動量を考慮した上で、フレーム毎に認識された地形が順次追加される。例えば、ステレオカメラ２を基準点とする場合、既に作成されている地形マップを移動体の移動量だけオフセットさせた上で、新たに認識された地形がマップに合成される。また、例えば、地形の計測開始位置を基準とする場合、新たに認識された地形を移動体の移動量だけオフセットさせた上で、それが地形マップに合成される。以上の処理を通じて、地形のマップデータは、地形マップ生成部１４によって随時更新され、更新されたマップデータがマップデータメモリ１１に格納される。
【００５７】
このように、本実施形態によれば、ステレオ画像に基づいて地形が認識され、ステレオ画像に基づいて移動体の移動量が検出される。そして、移動体の移動量を考慮した上で、地形の三次元的形状を示すマップに新たに認識された地形が随時追加されていく。その結果、移動体の移動に伴い、広範囲な地形マップを精度よく生成することができる。
【００５８】
また、移動体が通過した経路上の地形形状は地形マップとしてマップデータメモリ１１に記憶されるため、このデータを利用した移動制御を行えば、より高度な移動体の移動制御を行うことが可能となる。例えば、移動体が前方を向いたままバックするといった如くである。
【００５９】
また、本実施形態によれば、移動体の移動量をステレオ画像のみから算出することができる。したがって、移動体の姿勢や速度等を検出するセンサを別途追加する必要がなくなるため、センサ数の削減を図ることができる。
【００６０】
（第２の実施形態）
本実施形態の特徴は、図１に示した地形認識装置１の一部を構成する地形データ生成部１２を改良した点にある。具体的には、図３に示した地形データ生成部１２の構成に対象物ヒストグラム生成部２４を追加し、対象物の存在頻度分布を考慮した上で、地面高ｙｎｍを特定する点にある。図９は、第２の実施形態にかかる地形データ生成部１２のブロック構成図である。同図において、図３に示した構成ブロックと同一のものについては、同一の番号を付して、ここでの説明を省略する。
【００６１】
対象物ヒストグラム生成部２４は、三次元位置算出部２０によって算出された三次元位置（Ｘ，Ｙ，Ｚ）に基づいて、高さ方向における対象物の存在頻度分布を生成する。この頻度分布の生成は、視線通過の頻度分布と同様に、図５に示した区分Ｓｎｍ毎に行われる。図１０は、ある区分Ｓｎｍに関する対象物の存在頻度ヒストグラムの一例を示す図である。対象物の存在頻度分布は、図７に示した視線通過頻度ヒストグラムと同様に、高さｙを縦軸として、予め設定された高さ区間（例えば、０．１ｍ）毎に縦軸が区切られている。そして、ある対象物の三次元位置（Ｘ，Ｙ，Ｚ）が区分Ｓｎｍ内に存在する場合、その対象物が存在する高さ区間の度数に１を加算する。この加算処理は、区分Ｓｎｍ内に存在するすべての対象物を加算対象として行われる。これにより、それぞれの高さ区間における対象物の存在度数が算出される。
【００６２】
地形認識部２３は、視線通過の頻度分布と対象物の存在頻度分布とに基づいて、それぞれの区分Ｓｎｍにおける地面高ｙｎｍを特定する。具体的には、まず、第１の実施形態で説明した手法に従い、区分Ｓｎｍに関する視線通過の頻度分布より、視線ベースの地面高ｙ１ｎｍが算出される。つぎに、区分Ｓｎｍに関する対象物の存在頻度分布より、対象物ベースの地面高ｙ２ｎｍが算出される。この地面高ｙ２ｎｍは、所定の閾値Ｔｈ以上の度数を有する高さ区間のうち、最も低い高さ区間として特定することができる。例えば、図１０のケースでは、閾値Ｔｈ以上の高さ区間が７つ存在するが、その中で高さが最も低い区間（−０．３ｍ〜−０．２ｍ）が地面高ｙ２ｎｍとなる。そして、視線ベースの地面高ｙ１ｎｍと対象物ベースの地面高ｙ２ｎｍとに基づいて、最終的な地面高ｙｎｍが算出される。最終的な地面高Ｙｎｍは、例えば、視線ベースの地面高Ｙ１ｎと対象物ベースの地面高Ｙ２ｎｍとの単純平均または加重平均より算出することができる。この場合の加重平均の重みとして、例えば、対象物ベースの地面高Ｙ２ｎｍの信頼度を用いてもよい。すなわち、対象物ベースの地面高Ｙ２ｎｍの信頼度が大であれば、Ｙ２ｎｍの重みを増やし、これが小であれば、Ｙ２ｎｍの重みを増やすといった如くである。信頼度としては、例えば、区分Ｓｎｍに関する対象物の存在頻度のうち、閾値Ｔｈ以上である高さ区間における頻度の合計値と、所定の判定基準閾値との比を用いる方法がある。
【００６３】
このように、本実施形態によれば、第１の実施形態と同様の効果を有する。特に、本実施形態では、視線通過の頻度分布のみならず、対象物の存在頻度分布をも考慮して、地面高ｙｎｍを算出しているため、地面の三次元的形状を一層精度よく認識することが可能となる。
【００６４】
なお、対象物の存在頻度ヒストグラムにおいて、特定された地面高以下の頻度を除けば、地面上に存在する障害物のみを認識することも可能である。
【００６５】
【発明の効果】
このように、本発明によれば、移動体に搭載されたステレオカメラより得られるステレオ画像を処理することにより、地面の三次元的形状を精度よく認識でき、かつ、移動体の移動に伴い、地形認識を広範囲に亘って行うことが可能となる。また、ステレオ画像の処理を通じて、移動体の移動量を精度よく検出することができるため、移動体の運動状態を検出するセンサ数の削減を図ることが可能となる。
【図面の簡単な説明】
【図１】第１の実施形態にかかる地形認識装置のブロック構成図
【図２】基準画像に設定される画素ブロックＰＢｉｊの説明図
【図３】地形データ生成部のブロック構成図
【図４】三次元空間上に設定される視線Ｌの説明図
【図５】三次元空間上に設定される区分Ｓｎｍの説明図
【図６】区分Ｓｎｍを通過する視線Ｌ群の一例を示す図
【図７】区分Ｓｎｍに関する視線通過頻度ヒストグラムの一例を示す図
【図８】自己運動検出部のブロック構成図
【図９】第２の実施形態にかかる地形データ生成部のブロック構成図
【図１０】区分Ｓｎｍに関する対象物の存在頻度ヒストグラムの一例を示す図
【符号の説明】
１　　地形認識装置
２　　ステレオカメラ
２ａ　メインカメラ
２ｂ　サブカメラ
４　　Ａ／Ｄコンバータ
５　　Ａ／Ｄコンバータ
６　　画像補正部
７　　ステレオ画像処理部
８　　距離データメモリ
９　　画像データメモリ
１０　　マイクロコンピュータ
１１　　マップデータメモリ
１２　　地形データ生成部
１３　　自己運動検出部
１４　　地形マップ生成部
２０　　三次元位置算出部
２１　　視線設定部
２２　　視線ヒストグラム生成部
２３　　地形認識部
２４　　対象物ヒストグラム生成部
３０　　基準領域設定部
３１　　対応領域設定部
３２　　三次元位置算出部
３３　　移動量検出部[0001]
TECHNICAL FIELD OF THE INVENTION
The present invention relates to a terrain recognizing device and a terrain recognizing method for recognizing a three-dimensional shape of the ground, and more particularly to recognizing an undulating shape of the ground based on a stereo image obtained by a stereo camera mounted on a moving body. About technology. The present invention also relates to a method for detecting a moving amount of a moving object using a stereo image.
[0002]
[Prior art]
2. Description of the Related Art Conventionally, a technology for recognizing a three-dimensional shape of the ground using a stereo image has been proposed. For example, Patent Literature 1 discloses a technique of detecting a white line on a road by performing stereo matching on a stereo image as a processing target, and detecting a road surface from a three-dimensional position of the detected white line. Further, Patent Literature 2 discloses a technique of globally approximating the ground having few image features, such as a road and a floor, using a stereo image to recognize the ground as a plane.
[0003]
Further, Patent Literature 3 discloses a technique for recognizing an undulating shape of the ground based on a stereo image obtained by imaging a scene including the ground, regardless of the presence or absence of an obstacle on the ground. Specifically, first, a reference elevation value of the subject is calculated based on the elevation components of the three-dimensional points in the subject. Next, a three-dimensional point having an elevation component (projected elevation) exceeding the reference elevation value is detected as a projected position. The detected protruding position is regarded as a position where a protruding obstacle exists in the three-dimensional space. As for the altitude component of the protruding position, an average altitude value calculated from the altitude components around the protruding position is used instead of the protruding altitude. As described above, by correcting the elevation component at the protruding position where the obstacle exists, the undulating shape of the ground excluding the obstacle is specified.
[0004]
Among the prior applications filed by the present applicant, there is Japanese Patent Application No. 2002-184020 related to the present invention.
[0005]
On the other hand, Patent Literature 4 discloses a technique in which two stereo cameras are mounted on a moving object, and a self-position of the moving object is recognized using stereo images obtained from the stereo cameras. One stereo camera outputs a stereo image (distant image) in time-series frame units by imaging a distant landscape, and the other stereo camera outputs a stereo image (lower image) by imaging a lower landscape. Is output in time-series frame units. The movement of the moving object is detected based on the movement of the distant image between frames and the movement of the lower image between frames. The movement of the distant image and the movement of the lower image are converted into the amount of movement in a three-dimensional space (real space) based on each distance image by a known coordinate conversion formula. Then, the net translation speed component is calculated by removing the rotation speed component caused by the movement of the distant image from the speed component caused by the movement of the lower image. The self-position of the moving object is specified by converting it into a translation speed component based on the distance measurement start point and accumulating this component.
[0006]
[Patent Document 1]
JP-A-5-26547
[Patent Document 2]
JP-A-9-81755
[Patent Document 3]
JP-A-8-285586
[Patent Document 4]
JP-A-11-51650
[0007]
[Problems to be solved by the invention]
SUMMARY OF THE INVENTION It is an object of the present invention to accurately recognize a three-dimensional shape of the ground using a stereo camera mounted on a moving body, and to perform the terrain recognition over a wide range as the moving body moves. .
[0008]
Another object of the present invention is to accurately detect the amount of movement of a moving object using a stereo camera mounted on the moving object.
[0009]
[Means for Solving the Problems]
In order to solve such a problem, a first invention has a stereo camera, a stereo image processing unit, a terrain data generation unit, a self-motion detection unit, and a terrain map generation unit, and is mounted on a moving object. And a terrain recognition device for recognizing a three-dimensional shape of the ground. The stereo camera captures a scene including the ground in chronological order, and outputs a pair of image data for each frame. The stereo image processing unit calculates parallax by stereo matching based on the pair of image data, and outputs distance data in which the calculated parallax is associated with a position on an image plane defined by the image data for each frame. Output to The terrain data generation unit generates terrain data indicating a three-dimensional shape of the terrain in the first frame based on the distance data of the first frame. The self-motion detector detects the image data of the first frame, the image data of the second frame preceding the first frame, the distance data of the first frame, and the distance data of the second frame. Based on this, the movement amount of the moving body between the first frame and the second frame is detected. The terrain map generation unit generates a terrain map by sequentially combining the terrain data generated by the terrain data generation unit in consideration of the movement amount detected by the self-motion detection unit.
[0010]
Here, in the first aspect, the terrain data generation unit includes: a three-dimensional position calculation unit that calculates a three-dimensional position of each target object based on the first distance data; A line-of-sight setting unit that sets a line-of-sight group that connects each of the three-dimensional positions and the stereo camera mounting position, and by dividing the three-dimensional space into a matrix in two directions except for the height direction, In addition to setting a plurality of sections, based on the line of sight set by the line-of-sight setting unit, for each section, a line-of-sight histogram generation unit that generates a line-of-sight frequency distribution in the height direction, and a line-of-sight frequency distribution And a terrain recognition unit that specifies the height of the ground in each of the sections based on the information. The terrain data generation unit may further include an object histogram generation unit that generates an existence frequency distribution of the object in the height direction for each section based on each three-dimensional position of the object. . In this case, it is preferable that the terrain recognition unit specifies the height of the ground in each of the sections based on the line-of-sight frequency distribution and the object presence frequency distribution. Further, the terrain recognizing unit, when the height of the ground specified for a certain section is different from the height of the ground around the section by a predetermined threshold or more, calculates the height of the ground of the section and the height of the ground around the section. It is preferable to make correction based on this.
[0011]
Further, in the first invention, the self-momentum detection unit sets a plurality of reference regions on an image plane defined by image data of one of the first frame and the second frame, and sets the plurality of reference regions on the image plane. And a reference area setting unit for specifying a reference point indicating the position of the reference area, and a corresponding area having a correlation with each of the reference areas on an image plane defined by the other image data of the first frame or the second frame. A corresponding area setting unit for specifying a corresponding point indicating the position of the corresponding area on the image plane; and three reference points selected from a plurality of reference points specified by the reference area setting unit. The three-dimensional position of each point is calculated based on the distance data of one frame, and three corresponding points among a plurality of corresponding points specified by the corresponding region setting unit are calculated. Selected, the respective three-dimensional position of the selected corresponding point, a three-dimensional position calculation unit that calculates based on the distance data of the other frame, the three-dimensional position of the reference point calculated by the three-dimensional position calculation unit and The image processing apparatus may further include a movement amount detection unit that detects a movement amount of the moving body between the first frame and the second frame based on the three-dimensional position of the corresponding point. In this case, it is preferable that the three-dimensional position calculating unit calculates the three-dimensional position of the selected reference point after evaluating the positional relationship of the selected reference point.
[0012]
A second invention provides a terrain recognition method that is mounted on a moving body and recognizes a three-dimensional shape of the ground. In this terrain recognition method, a stereo camera captures a scene including the ground in chronological order and outputs a pair of image data for each frame, and calculates parallax by stereo matching based on the pair of image data. Outputting, for each frame, distance data in which the calculated parallax and the position on the image plane defined by the image data are associated with each other; and a first frame based on the distance data of the first frame. Generating terrain data indicating a three-dimensional shape of the terrain in the first frame, image data of a first frame, image data of a second frame preceding the first frame, and distance data of the first frame Based on the distance data of the second frame and the distance data of the second frame. And-up, in consideration of the detected movement amount, by sequentially synthesizing the topography data generated for each frame, and generating a topographic map.
[0013]
According to a third aspect of the present invention, stereo matching processing is performed on the image data of the first and second frames and the image data of the first frame, which are obtained by capturing a scene including the ground in a time series with a stereo camera. Based on the distance data of the first frame obtained and the distance data of the second frame obtained by performing the stereo matching processing on the image data of the second frame preceding the first frame, A moving amount detecting method for detecting a moving amount is provided. This moving amount detection method sets a plurality of reference areas on an image plane defined by image data of one of a first frame and a second frame, and sets a reference point indicating a position of the reference area on the image plane. Specifying and setting a corresponding area having a correlation with each of the reference areas on an image plane defined by the other image data of the first frame or the second frame, and setting a position of the corresponding area on the image plane And selecting three reference points from the plurality of specified reference points, and determining the three-dimensional position of each of the selected reference points based on the distance data of one frame. In addition to the calculation, three corresponding points are selected from the plurality of specified corresponding points, and the three-dimensional positions of the selected corresponding points are determined based on the distance data of the other frame. Calculating it is, based on the three-dimensional position of the corresponding point and three-dimensional position of the reference point, and a step of detecting an amount of movement of the moving body between the first and second frames.
[0014]
BEST MODE FOR CARRYING OUT THE INVENTION
(1st Embodiment)
FIG. 1 is a block diagram of the terrain recognition device according to the first embodiment. The terrain recognition device 1 recognizes the three-dimensional shape of the undulating ground regardless of whether an obstacle exists on the ground. The stereo camera 2 is mounted on a moving object (agriculture and forestry work vehicle, civil engineering work vehicle, or exploration vehicle, etc.) that moves on the ground, and images a scene including the ground in a time-series manner with a predetermined depression angle. The stereo camera 2 is composed of a pair of cameras 2a and 2b, and outputs a pair of images (stereo images) necessary for performing the subsequent stereo image processing in frame units. Each of the cameras 2a and 2b has a built-in image sensor such as a CCD or a CMOS sensor. The main camera 2a captures a reference image (right image), and the sub camera 2b captures a comparison image (left image). In a state where they are synchronized with each other, the analog images output from the cameras 2a and 2b are converted into digital images of a predetermined luminance gradation (for example, 256 gray scales) by the A / D converters 4 and 5. Is converted.
[0015]
The stereo image output from the stereo camera 2 in frame units is subjected to image processing such as luminance correction and coordinate conversion in the image correction unit 6. Normally, the mounting positions of the pair of cameras 2a and 2b are different in degree but have an error, so that a shift due to the error occurs in each of the left and right images. In order to equivalently correct such a shift by image processing, the image correction unit 6 performs a geometric conversion such as rotation and translation of the image. Further, optical distortion of the cameras 2a and 2b is also equivalently corrected by the image processing in the image correcting unit 6.
[0016]
Through such image processing, reference image data is obtained from the main camera 2a, and comparison image data is obtained from the sub camera 2b. Each image data is a set of pixel luminance values (0 to 255). Here, the image plane defined by the image data is expressed by an ij coordinate system, with the lower left corner of the image as the origin, the horizontal direction as the i axis, and the vertical direction as the j axis. Stereo image data corresponding to one frame, which is a display unit of one image, is output to the stereo image processing unit 7 at the subsequent stage and stored in the image data memory 9.
[0017]
Note that the image data memory 9 can store at least two frames of stereo image data Pn and Pn-1 because the self-position of the moving object is detected based on the motion of the image between frames, as described later. Has storage capacity. Hereinafter, the reference image data Pn for one frame obtained at the current imaging timing is referred to as “first frame image data Pn” as appropriate, and at the imaging timing earlier (typically immediately before). The obtained reference image data Pn-1 for one frame is appropriately referred to as "second frame image data Pn-1".
[0018]
The stereo image processing unit 7 calculates distance data by stereo matching processing based on the reference image data and the comparison image data. Here, the “distance data” is a set of parallaxes d calculated for each small area in the image plane defined by the reference image data, and each parallax d is a position (i, j) on the image plane. Corresponding. One parallax d is calculated from a pixel block having a predetermined area (for example, 4 × 4 pixels) that forms a part of the reference image, and thus the pixel block is a unit for calculating the parallax d.
[0019]
FIG. 2 is an explanatory diagram of a pixel block set in the reference image. For example, when the reference image is composed of 200 × 512 pixels, a parallax group corresponding to the number of pixel blocks PBij (50 × 128) can be calculated from a captured image corresponding to one frame. As is well known, the parallax d is the amount of displacement of the pixel block PBij, which is the calculation unit, in the horizontal direction, and has a large correlation with the distance to the object projected in the pixel block PBij. In other words, the closer the object shown in the pixel block PBij is to the stereo camera 2, the larger the parallax d of the pixel block PBij becomes, and the farther the object is, the smaller the parallax d becomes. , The parallax d becomes 0).
[0020]
When calculating the parallax d of a certain pixel block PBij (correlation source), an area (correlation destination) having a correlation with the luminance characteristic of this pixel block PBij is specified in the comparison image. As described above, the distance from the stereo camera 2 to the target is reflected in the amount of horizontal displacement between the reference image and the comparison image. Therefore, when searching for the correlation destination in the comparison image, it is not necessary to search the entire comparison image, but it is sufficient to search on the same horizontal line (epipolar line) as the j coordinate of the pixel block Pij as the correlation source. The stereo image processing unit 7 shifts the pixel on the epipolar line by one pixel within a predetermined search range set on the basis of the i coordinate of the correlation source, and determines the correlation between the correlation source and the correlation destination candidate. Evaluate sequentially (stereo matching). Then, in principle, the horizontal shift amount of the correlation destination (one of the correlation destination candidates) determined to have the highest correlation is the parallax d of the pixel block PBij.
[0021]
The correlation between two pixel blocks can be evaluated, for example, by calculating a city block distance CB. Equation 1 shows the basic form of the city block distance CB. In the equation, p1ij is the luminance value of the ij-th pixel of one pixel block, and p2ij is the ij-th luminance value of the other pixel block. The city block distance CB is the total sum of the difference (absolute value) between the two luminance values p1ij and p2ij corresponding to the position in the entire pixel block. The smaller the difference, the greater the correlation between the two pixel blocks. .
(Equation 1)
CB = Σ | p1ij−p2ij |
[0022]
Basically, among the city block distances CB calculated for each pixel block existing on the epipolar line, the pixel block having the minimum value is determined as the correlation destination. Then, the shift amount between the correlation destination and the correlation source specified in this manner becomes the parallax d. The hardware configuration of the stereo image processing unit 7 for calculating the city block distance CB is disclosed in Japanese Patent Application Laid-Open No. H5-114099. The distance data (i, j, d) calculated through the above processing is stored in the distance data memory 8.
[0023]
Note that the distance data memory 8 stores at least two frames of stereo distance data Dn, Dn− in order to detect the self-position of the moving object based on the movement of the three-dimensional position of the object between frames, as described later. 1 can be stored. Hereinafter, the distance data Dn calculated from the image data Pn of the first frame is appropriately referred to as “distance data Dn of the first frame”, and the distance data Dn− calculated from the image data Dn−1 of the second frame. 1 is appropriately referred to as “distance data Dn−1 of the second frame”.
[0024]
The microcomputer 10 includes a CPU, a ROM, a RAM, an input / output interface, and the like. When the microcomputer 10 is functionally grasped, it has a terrain data generation unit 12, a self-motion detection unit 13, and a terrain map generation unit 14. . Although the processing contents of the individual functional blocks 12 to 14 will be described later, the map data representing the three-dimensional shape of the terrain is stored in the distance data of a plurality of frames stored in the distance data memory 8 and the image data memory 9. It is generated based on the image data of a plurality of frames and is updated as needed in a time series.
[0025]
The terrain data generation unit 12 generates and outputs terrain data Gn in the first frame based on the distance data Dn of the first frame. The terrain data Gn is data representing the three-dimensional shape of the terrain projected within the field of view of the stereo camera 2 in the first frame. FIG. 3 is a block diagram of the terrain data generation unit 12. The terrain data generation unit 12 includes a three-dimensional position calculation unit 20, a sight line setting unit 21, a sight line histogram generation unit 22, and a terrain recognition unit 23.
[0026]
The three-dimensional position calculation unit 20 reads the distance data (i, j, d) of the first frame stored in the distance data memory 8, and calculates the three-dimensional position (X, Y, Z) of each object. calculate. The three-dimensional position (X, Y, Z) indicating the position of the object in the three-dimensional space is based on a well-known coordinate conversion formula as shown in Expression 2 when the distance directly below the main camera 2a is used as a reference. It is uniquely specified from (i, j, d). The three-dimensional position is represented by an XYZ coordinate system, with the ground O right below the center of the stereo camera 2 (main camera 2a in the present embodiment) as the origin O, the horizontal direction (camera base line direction) as the X axis, and the vertical direction. The direction (height direction) is the Y axis, and the front-rear direction (distance direction) is the Z axis. In the equation, the constant KZH is (camera base line length / horizontal viewing angle per pixel). The constant CAH is the height at which the stereo camera is mounted, the constant PWV is the vertical viewing angle per pixel, and the constant PWH is the horizontal viewing angle per pixel. Further, the constant IV is an i coordinate value of the vanishing point V set in advance, and the constant JV is its j coordinate value.
(Equation 2)

[0027]
The line-of-sight setting unit 21 is based on the three-dimensional position (X, Y, Z) calculated by the three-dimensional position calculation unit 20, and sets a line of sight L that connects the mounting position of the stereo camera 2 and the position of the object in the three-dimensional space. Set. FIG. 4 is an explanatory diagram of the line of sight L set in a three-dimensional space defined by the X axis, the Y axis, and the Z axis. The line of sight L regarding a certain object B (including both the ground and the obstacle) is represented by the three-dimensional position (X, Y, Z) of the object B and the mounting position C (0, y1, 0) of the stereo camera 2. , And the path of the line of sight L to the object B is uniquely specified by this equation. Here, y1 is the mounting height of the stereo camera 2 with reference to the ground immediately below the stereo camera 2. The line-of-sight setting unit 21 sequentially calculates the line-of-sight L for each of the objects B with all the objects B as the calculation targets. The number of visual line L groups calculated in one processing cycle is equivalent to the number of target objects B, in other words, to the number of parallax d groups constituting distance data of one frame.
[0028]
The eye-gaze histogram generation unit 22 sets a section Snm, which is a unit for calculating the ground height, on a three-dimensional space. FIG. 5 is an explanatory diagram of the section Snm set on the three-dimensional space. The three-dimensional space is divided into a matrix in two directions except the height direction, that is, divided into N in the X direction and M in the Z direction. Thus, N × M square pillar-shaped sections Snm are defined in the three-dimensional space. The resolution of the terrain map depends on the size of the bottom area (cross-sectional area) of one section Snm, and the smaller this is, the higher the resolution of the terrain map (resolution in top view) is. However, if the bottom area of the section Snm is too small, the accuracy of calculating the ground height may be reduced. This is because the number of the lines of sight L passing through one section Snm (the number of samples) decreases when the bottom area of the section Snm is reduced due to the determination of the ground height by the statistical processing of the line of sight L group. . Therefore, the bottom area of the section Snm (in other words, the number of divisions of the three-dimensional space) needs to be determined in consideration of both the resolution required for the terrain map and the calculation accuracy of the ground height. In the present embodiment, the bottom shape of each section Snm is set to 20 cm square as an example. In this way, by dividing the three-dimensional space into a matrix in two directions excluding the height direction, a plurality of three-dimensional sections Snm are set in a grid on the three-dimensional space. In one section Snm, for example, the line of sight L exists along a passing path as shown in FIG. The fact that many lines of sight L pass through a certain section Snm means that there is no object (obstacle) that blocks the line of sight within that section Snm.
[0029]
Setting the quadrangular prism-shaped section Snm by dividing the three-dimensional space into a lattice shape is an example, and the setting method and the three-dimensional shape of the section Snm are not limited to this. For example, the section Snm may be a hexagonal prism.
[0030]
The gaze histogram generation unit 22 generates a gaze passage frequency distribution in the height direction based on the gaze L group set by the gaze setting unit 21. The generation of the frequency distribution is performed for each section Snm. FIG. 7 is a diagram illustrating an example of a line-of-sight passage frequency histogram for a certain section Snm. As shown in the drawing, the line-of-sight passage frequency distribution is, for example, with the height y as the vertical axis, and the vertical axis is divided for each preset height section (for example, 0.1 m). The resolution in the height direction of the terrain map depends on the width of the height section per section. Each time one line of sight L passes through a certain height section, 1 is added to the frequency of that section. Such addition of the number of passes of the line of sight L is performed for all the lines of sight L existing in the section Snm. Thereby, the frequency in each height section is obtained, and the frequency distribution of gaze passage is calculated.
[0031]
The terrain recognition unit 23 specifies the ground height ynm in the section Snm based on the frequency distribution of the line of sight passage for the section Snm. As a method for specifying the ground height ynm, for example, the following three methods can be considered.
[0032]
The first method is a method of determining the ground height ynm based on the frequency of each height (frequency of appearance of the line of sight) while giving priority to the lower height. Specifically, in the line-of-sight passage frequency distribution, among the height sections having a frequency equal to or greater than the predetermined threshold Th, the lowest height section or the height section immediately below (one below) is classified into the section Snm. Is the ground height ynm. For example, in the case of FIG. 7, there are seven height sections that are equal to or greater than the threshold Th, and among them, the section having the lowest height (−0.2 m to −0.1 m) or the height immediately below it The section (−0.3 m to −0.2 m) is the ground height ynm.
[0033]
When the moving body attempts to recognize the surrounding terrain for its own movement, it is desirable to observe the ground as wide as possible and to grasp the surrounding terrain globally. In the present embodiment, the ground height is specified by examining the line of sight, that is, the frequency of passage of a virtual straight line connecting the stereo camera 2 and the object in the three-dimensional space. This method of determining the height of the ground is based on the knowledge that there are no obstacles in the space where the distant view is visible, and enables the recognition of terrain in a wide range including distant places where the distance information of the ground itself is sparse . When many lines of sight L pass through a certain section Snm, it means that there is no object that blocks the line of sight L within the section Snm. Based on such knowledge, in the first method, the height sections are sequentially examined from the bottom to the top, and the height section having a frequency equal to or greater than the threshold Th first, or the height immediately below the height section The interval is assumed to be at ground level ynm.
[0034]
The second technique is based on the same knowledge as the first technique, but is a technique of determining the ground height ynm based on the magnitude of the cumulative frequency counted from the lower one. Specifically, in the frequency distribution of the line of sight passage, the cumulative frequency is calculated by accumulating the frequency of the line of sight from the lower side to the higher side. Then, a height section in which the cumulative frequency is equal to or more than a predetermined threshold value or a height section immediately below the accumulated frequency is specified as the ground height ynm.
[0035]
The third method is a method of determining the ground height ynm by grouping heights having high frequencies among the heights adjacent in the height direction. Specifically, in the line-of-sight passage frequency distribution, an area in which height sections in which the frequency is equal to or more than a predetermined threshold is continuous in the height direction is determined to be a space area where no object is present. In addition, a region in which the height section in which the frequency is less than the threshold is continuous in the height direction is determined to be a non-spatial region where the target object exists. All height sections are divided into either spatial or non-spatial regions. Among the space regions specified as described above (a plurality of space regions may be specified), the space region having the largest range in the height direction is determined to be the space on the ground. Then, on the basis of the lowest height section in the space area corresponding to the space on the ground, the height section immediately below is specified as the ground height ynm. For example, in the case of FIG. 7, seven height sections (−0.2 to 0.5) that are equal to or greater than the threshold Th (different from the threshold Th of the first method) are determined to be spaces on the ground, and among them, Is the lowest height section (−0.2 to −0.1). Therefore, the ground height ynm is (-0.3 to -0.2) corresponding to the height section immediately below the height section (-0.2 to -0.1). The third method is effective when there is a hole in the ground or when there is a large number of mismatches in stereo matching, as compared with the first and second methods.
[0036]
If a certain section Snm does not satisfy the ground-specific requirements in the above-described method, the terrain recognition section 23 estimates the ground height ynm of the section Snm based on the surrounding ground height ynm '. For example, the ground height ynm is determined by an interpolation process of interpolating based on the surrounding ground height ynm '.
[0037]
The terrain data Gn of the first frame calculated by the terrain recognition unit 23 is output to the subsequent terrain map generation unit 14. The terrain data Gn is data representing the three-dimensional shape of the ground projected in the field of view of the stereo camera 2 in the first frame. Specifically, the section Snm set in the first frame is This is a set of ground heights ynm as calculation units.
[0038]
As described above, the terrain data generation unit 12 divides the three-dimensional space into a matrix in two directions (the X direction and the Z direction) except for the height direction (the Y direction), and thereby, a plurality of divisions are performed on the three-dimensional space. Set Snm. For each section Snm, a passing frequency distribution of the line of sight L in the height direction is generated. Then, in each section Snm, the ground height ynm is determined by examining the statistical passage path of the line of sight L. This makes it possible to specify the ground height Ynm even in a situation with undulations and irregularities such as irregular terrain, or in a distant place where it is difficult to obtain distance data of the ground itself within the field of view. it can. As a result, since the ground height Ynm can be calculated stably and accurately in a wide range, it is possible to accurately recognize the three-dimensional shape of the undulating ground.
[0039]
In calculating the ground height ynm, not only the information in the corresponding section Snm but also the statistics such as the total and average of the frequencies in the section Snm ′ within a predetermined range surrounding the section Snm. You may decide.
[0040]
In addition, the terrain recognition unit 23 may specify the ground height ynm in consideration of the continuity of the surrounding ground height ynm 'to the height specified from the above-mentioned line-of-sight frequency distribution. For example, if the ground height ynm specified for a certain section Snm is different from the ground height ynm 'around the section Snm by a predetermined threshold or more, the ground height ynm is corrected (leveled) based on the surrounding ground height ynm'. ). This makes it possible to more accurately recognize the three-dimensional shape of the terrain.
[0041]
On the other hand, the self-motion detecting unit 13 determines the distance between the previous and next frames based on the image data Pn and Pn-1 of the first and second frames and the distance data Dn and Dn-1 of the first and second frames. , The moving amount of the moving body itself (in other words, the moving amount of the stereo camera 2) is detected, and the moving amount data Mn representing the moving state (moving state) of the moving body between the previous and next frames is generated and output. The moving state of the moving body is defined by three rotation components (yaw, roll, pitch) and three translation components (X direction, Y direction component, and Z direction component). FIG. 8 is a block diagram of the self-motion detecting unit 13. The self-motion detecting unit 13 includes a reference region setting unit 30, a corresponding region setting unit 31, a three-dimensional position calculating unit 32, and a movement amount detecting unit 33.
[0042]
First, the reference region setting unit 30 sets i (for example, 48) reference regions Ri on the image plane defined by the image data Pn-1 of the second frame. These reference regions Ri are set to be dispersed as much as possible over the entire image plane, and each reference region Ri has an area of 8 × 8 pixels. A point to be noted when setting the reference region Ri is that an “appropriate region” for performing two-dimensional matching should be set. The “suitable region” is a region having a luminance edge, specifically, a region in which luminance change between adjacent pixels in the region is large. Therefore, when setting the reference region Ri on the image plane of the second frame, as a prerequisite, an evaluation is made as to whether or not the region is a “suitable region” for performing two-dimensional matching. In the present embodiment, as an example, the condition of the reference region Ri is that the sum of the sum of the brightness differences in the horizontal direction and the sum of the brightness differences in the vertical direction is equal to or greater than a predetermined threshold.
[0043]
Specifically, in a region that is a candidate for the reference region Ri, a luminance change amount (absolute value) Δp1 is calculated for each pixel pair adjacent in the horizontal direction. Since the reference region Ri has an area of 8 × 8 pixels, 64 luminance change amounts Δp1 are calculated. At this time, for the rightmost pixel column (or the leftmost pixel column), the luminance change amount Δp1 is calculated using an adjacent pixel column outside the region. The sum of these 64 luminance change amounts Δp1 is referred to as a horizontal luminance sum PA1. Next, with respect to this candidate area, a luminance change amount (absolute value) Δp2 is calculated for each pair of pixels adjacent in the vertical direction. Since the reference region Ri has an area of 8 × 8 pixels, 64 luminance change amounts Δp2 are calculated. At this time, for the lowermost pixel row (or the uppermost pixel row), the luminance change amount Δp2 is calculated using an adjacent pixel row outside the region. The sum of these 64 luminance change amounts Δp2 is referred to as a vertical luminance sum PA2. Then, the sum of the horizontal luminance sum PA1 and the vertical luminance sum PA2 is obtained, and when this sum (PA1 + PA2) is larger than a predetermined threshold, this area is set as the reference area Ri.
[0044]
By repeatedly setting and evaluating such candidate areas, i reference areas Ri are set on the image plane of the second frame. When the i reference regions Ri are set, the respective positions on the image plane are output to the subsequent three-dimensional position calculator 32. The position of the reference region Ri is specified by the reference point ai. For example, the coordinates (i, j) at the lower left of the reference region Ri are set as the reference point ai.
[0045]
On the other hand, the corresponding area setting unit 31 sets a corresponding area Ci having a luminance correlation with each reference area Ri on an image plane defined by the image data Pn of the first frame. For this purpose, the corresponding area setting unit 31 sets the two-dimensional city block distance CB between the reference area Ri and a pixel block of 8 × 8 pixels (candidate of the corresponding area Ci) set within a predetermined search range. To find a suitable distribution. The distribution of the city block distance CB is generated by performing the above-described calculation of Expression 1 for each pixel block existing within the search range. One city block distance CB is calculated for each comparison object while offsetting the comparison object by one pixel in the horizontal / vertical directions over the entire search range (two-dimensional matching). Then, with reference to the two-dimensional distribution, among the candidates for the corresponding region Ci, the candidate having the minimum value of the city block distance CB is set as the corresponding region Ci.
[0046]
By performing such a correlation evaluation for each reference region Ri, a corresponding region Ci corresponding to the number of reference regions Ri is specified on the image plane of the first frame. When the corresponding areas Ci are set, the positions on the image plane relating to the corresponding areas are output to the three-dimensional position calculating unit 32 at the subsequent stage. The position of the corresponding area Ci is specified by the corresponding point bi, and as an example, the lower left coordinate (i, j) of the corresponding area Ci is set as the corresponding point bi.
[0047]
The evaluation of the luminance edge and the two-dimensional matching described above are described in detail in Japanese Patent Application No. 11-261438 (Japanese Patent Application Laid-Open No. 2001-82955), which is a prior application of the present applicant. I want to be.
[0048]
In this embodiment, an example has been described in which the reference region Ri is set on the image plane of the second frame and the corresponding region Ci is set on the image plane of the first frame. It may be reversed. In this case, the reference area Ri is set on the image plane of the first frame, and the corresponding area Ci is set on the image plane of the second frame.
[0049]
The three-dimensional position calculation unit 32 selects at least three arbitrary points from the i reference points ai set on the image plane by an appropriate method, and evaluates the positional relationship between the selected three points. Then, the position in each three-dimensional space is calculated. In the present embodiment, it is evaluated whether the selected three reference points ai satisfy both of the following two evaluation conditions.
[0050]
The first evaluation condition is that the lengths of two line segments defined by the three reference points ai on the image plane are each equal to or greater than a predetermined threshold. As described later, in the present embodiment, the moving amount of the moving object is calculated based on a change between a triangle defined by at least three reference points ai and a triangle defined by three corresponding points bi. Therefore, if the lengths of the sides forming the triangle are too short, an effective triangle cannot be set in the three-dimensional space, and the error in the calculated movement amount increases. Therefore, the length of two sides set with reference to a certain reference point ai is evaluated according to the present evaluation condition.
[0051]
The second evaluation condition is that an angle between two vectors defined by three reference points ai in a three-dimensional space is 45 ° or more and 135 ° or less. If the three reference points ai do not satisfy this condition, the shape defined by these reference points ai is close to a straight line, so that an effective triangle cannot be set in the three-dimensional space. Therefore, in order to secure the calculation accuracy of the movement amount, the shape of the triangle is evaluated based on the evaluation conditions.
[0052]
For each of the reference points ai satisfying the above two conditions, the three-dimensional position calculation unit 32 calculates a three-dimensional space based on a set of the position (i, j) on the image plane and the corresponding parallax d. The upper position (X, Y, Z) is calculated. Here, as the parallax d, for example, an average parallax of a parallax group included in the reference region Ri in the distance data Dn-1 of the second frame can be used. Further, the coordinate conversion from the set of (i, j, d) to the three-dimensional position (X, Y, Z) is performed based on Equation 2 described above. Thereby, the positions (X, Y, Z) of the three reference points ai in the three-dimensional space are calculated. When three points in the three-dimensional space are specified, a triangle in the three-dimensional space is uniquely specified. Therefore, the positional relationship and the posture relationship between the triangle and the stereo camera 2 in the second frame are specified.
[0053]
In addition, the three-dimensional position calculation unit 32 calculates, for each of the corresponding points bi corresponding to the reference points ai satisfying the above two conditions, the position (i, j) on the image plane and the parallax d corresponding thereto. Based on the set, the position (X, Y, Z) in the three-dimensional space is calculated according to Equation 2. Here, as the disparity d, for example, an average disparity of a disparity group included in the corresponding area Ci in the distance data Dn of the first frame can be used. Thereby, the three-dimensional positions (X, Y, Z) of the three corresponding points ai are calculated, and the positional relationship and the posture relationship between the triangle and the stereo camera 2 in the first frame are specified.
[0054]
The movement amount detection unit 33 calculates the distance between the previous and next frames based on the three-dimensional positions (X, Y, Z) of the three reference points ai and the three-dimensional positions (X, Y, Z) of the three corresponding areas Ci. The moving amount of the moving object at is detected. As is well known, when three points in the three-dimensional space are specified, the triangle in the real space is uniquely specified, so that six components that define the degree of freedom of the moving object can be calculated. The six components are three rotation components (yaw, roll, pitch) and three translation components (X-direction component, Y-direction component, and Z-direction component). These components are calculated using a well-known calculation method, and output to the subsequent terrain map generation unit 14 as movement amount data Mn indicating the movement amount of the moving body.
[0055]
Note that the three-dimensional position calculation unit 32 repeats the setting of the set of three reference points ai and the calculation of the three-dimensional position a predetermined number of times in the same frame in order to increase the detection accuracy of the moving amount of the moving object, and repeats the necessary sample It is preferable to calculate only the number. In this case, it is preferable that the movement amount detection unit 33 generates the histogram for each of the six components calculated for each number of samples, and specifies the component having the highest frequency of occurrence to generate the movement amount data Mn. .
[0056]
The terrain map generator 14 updates the map data indicating the three-dimensional shape of the terrain based on the terrain data Gn from the terrain data generator 12 and the movement amount data Mn from the self-motion detector 13. Specifically, the terrain recognized for each frame is sequentially added to the already created terrain map in consideration of the moving amount of the moving object. For example, when the stereo camera 2 is used as a reference point, a newly recognized terrain is combined with the already created terrain map after offsetting the already created terrain map by the moving amount of the moving object. In addition, for example, when the measurement start position of the terrain is used as a reference, the newly recognized terrain is offset by the moving amount of the moving object, and is then combined with the terrain map. Through the above processing, the map data of the terrain is updated as needed by the terrain map generation unit 14, and the updated map data is stored in the map data memory 11.
[0057]
As described above, according to the present embodiment, the terrain is recognized based on the stereo image, and the moving amount of the moving object is detected based on the stereo image. Then, in consideration of the moving amount of the moving object, the newly recognized terrain is added to the map indicating the three-dimensional shape of the terrain as needed. As a result, a wide area terrain map can be accurately generated with the movement of the moving object.
[0058]
In addition, since the terrain shape on the route along which the moving object has passed is stored in the map data memory 11 as a terrain map, it is possible to perform more advanced movement control of the moving object by performing movement control using this data. It becomes. For example, the moving body is backed while facing forward.
[0059]
Further, according to the present embodiment, it is possible to calculate the moving amount of the moving object only from the stereo image. Accordingly, it is not necessary to separately add a sensor for detecting the posture, speed, and the like of the moving body, and thus the number of sensors can be reduced.
[0060]
(Second embodiment)
The feature of this embodiment lies in that the terrain data generation unit 12 constituting a part of the terrain recognition device 1 shown in FIG. More specifically, an object histogram generation unit 24 is added to the configuration of the terrain data generation unit 12 shown in FIG. 3, and the ground height ynm is specified in consideration of the object frequency distribution. FIG. 9 is a block diagram of the terrain data generation unit 12 according to the second embodiment. In the figure, the same components as those shown in FIG. 3 are denoted by the same reference numerals, and description thereof will be omitted.
[0061]
The object histogram generation unit 24 generates an object frequency distribution in the height direction based on the three-dimensional position (X, Y, Z) calculated by the three-dimensional position calculation unit 20. This frequency distribution is generated for each section Snm shown in FIG. 5, similarly to the frequency distribution of the line of sight passage. FIG. 10 is a diagram illustrating an example of an object presence frequency histogram for a certain section Snm. Similar to the line-of-sight passing frequency histogram shown in FIG. 7, the presence frequency distribution of the target object has the height y as the vertical axis, and the vertical axis is divided every predetermined height section (for example, 0.1 m). ing. Then, when the three-dimensional position (X, Y, Z) of a certain object exists in the section Snm, 1 is added to the frequency of the height section where the object exists. This addition process is performed for all objects existing within the section Snm as addition targets. Thereby, the frequency of presence of the target object in each height section is calculated.
[0062]
The terrain recognition unit 23 specifies the ground height ynm in each section Snm based on the line-of-sight frequency distribution and the target object existence frequency distribution. Specifically, first, the line-of-sight-based ground height y1 nm is calculated from the line-of-sight frequency distribution for the section Snm according to the method described in the first embodiment. Next, an object-based ground height y2 nm is calculated from the existence frequency distribution of the object with respect to the section Snm. The ground height y2 nm can be specified as the lowest height section among height sections having a frequency equal to or higher than the predetermined threshold Th. For example, in the case of FIG. 10, there are seven height sections equal to or greater than the threshold Th, and the section having the lowest height (−0.3 m to −0.2 m) is the ground height y2 nm. Then, the final ground height ynm is calculated based on the line-of-sight-based ground height y1 nm and the object-based ground height y2 nm. The final ground height Ynm can be calculated, for example, from a simple average or a weighted average of the line-of-sight-based ground height Y1n and the object-based ground height Y2nm. As the weight of the weighted average in this case, for example, the reliability of the ground height Y2 nm based on the object may be used. That is, if the reliability of the ground height Y2 nm based on the object is large, the weight of Y2 nm is increased, and if the reliability is small, the weight of Y2 nm is increased. As the reliability, for example, there is a method of using a ratio of a total value of frequencies in a height section that is equal to or greater than the threshold Th to a predetermined determination reference threshold among the frequencies of presence of the object related to the section Snm.
[0063]
As described above, according to the present embodiment, the same effects as those of the first embodiment are obtained. In particular, in the present embodiment, since the ground height ynm is calculated in consideration of not only the frequency distribution of the line of sight passage but also the frequency distribution of the object, the three-dimensional shape of the ground is recognized with higher accuracy. It becomes possible.
[0064]
It should be noted that it is also possible to recognize only obstacles existing on the ground, except for the frequency below the specified ground height in the presence frequency histogram of the object.
[0065]
【The invention's effect】
Thus, according to the present invention, by processing a stereo image obtained from a stereo camera mounted on a moving object, it is possible to accurately recognize the three-dimensional shape of the ground, and with the movement of the moving object, Terrain recognition can be performed over a wide range. In addition, since the amount of movement of the moving object can be accurately detected through the processing of the stereo image, the number of sensors for detecting the motion state of the moving object can be reduced.
[Brief description of the drawings]
FIG. 1 is a block configuration diagram of a terrain recognition device according to a first embodiment;
FIG. 2 is an explanatory diagram of a pixel block PBij set in a reference image.
FIG. 3 is a block diagram of a terrain data generation unit.
FIG. 4 is an explanatory diagram of a line of sight L set on a three-dimensional space;
FIG. 5 is an explanatory diagram of a section Snm set in a three-dimensional space.
FIG. 6 is a diagram showing an example of a line of sight L passing through a section Snm.
FIG. 7 is a diagram showing an example of a line-of-sight passing frequency histogram regarding a section Snm.
FIG. 8 is a block diagram of a self-motion detector.
FIG. 9 is a block diagram of a terrain data generation unit according to the second embodiment;
FIG. 10 is a diagram showing an example of an object presence frequency histogram related to a section Snm;
[Explanation of symbols]
1 Terrain recognition device
2 Stereo camera
2a Main camera
2b sub camera
4 A / D converter
5 A / D converter
6 Image correction unit
7 Stereo image processing unit
8 Distance data memory
9 Image data memory
10. Microcomputer
11 Map data memory
12 Terrain data generator
13 Self-motion detector
14 Terrain map generator
20 3D position calculator
21 Gaze setting unit
22 Eye-gaze histogram generator
23 Terrain recognition unit
24 Object histogram generator
30 Reference area setting section
31 Corresponding area setting section
32 3D position calculation unit
33 Movement detector

Claims (8)

In a terrain recognition device that is mounted on a moving object and recognizes the three-dimensional shape of the ground,
A stereo camera that captures a scene including the ground in chronological order and outputs a pair of image data for each frame;
Based on the pair of image data, a parallax is calculated by stereo matching, and distance data in which the calculated parallax is associated with a position on an image plane defined by the image data is output for each frame. A stereo image processing unit,
A terrain data generation unit that generates terrain data indicating a three-dimensional shape of the terrain in the first frame based on the distance data of the first frame;
Based on the image data of the first frame, the image data of the second frame preceding the first frame, the distance data of the first frame, and the distance data of the second frame. A self-motion detecting unit that detects an amount of movement of the moving body between the first frame and the second frame;
A terrain map generation unit that generates a terrain map by sequentially combining the terrain data generated by the terrain data generation unit in consideration of the movement amount detected by the self-motion detection unit. A terrain recognition device characterized by the following. The terrain data generation unit includes:
A three-dimensional position calculating unit that calculates a three-dimensional position of each object based on the first distance data;
In a three-dimensional space, a line-of-sight setting unit that sets a line-of-sight group connecting the three-dimensional position of each of the objects and the mounting position of the stereo camera.
In two directions excluding the height direction, by dividing the three-dimensional space into a matrix, a plurality of sections are set on the three-dimensional space, and based on the line of sight set by the line of sight setting section, A gaze histogram generation unit that generates a frequency distribution of gaze passage in the height direction,
The terrain recognition device according to claim 1, further comprising: a terrain recognition unit that specifies a height of the ground in each of the sections based on the frequency distribution of the line of sight passage. The terrain data generation unit includes:
Based on the three-dimensional position of each of the objects, for each of the sections, further includes an object histogram generation unit that generates a presence frequency distribution of the objects in the height direction,
The terrain according to claim 1, wherein the terrain recognition unit specifies the height of the ground in each of the sections based on the line-of-sight frequency distribution and the object presence frequency distribution. Recognition device. The terrain recognition unit, if the height of the ground identified for a certain section is different from the height of the ground around the section by a predetermined threshold or more, the height of the ground of the section, the ground around the section The terrain recognition device according to claim 2, wherein the correction is performed based on the height of the terrain. The self-momentum detector,
A reference for setting a plurality of reference areas on an image plane defined by image data of one of the first frame and the second frame and specifying a reference point indicating a position of the reference area on the image plane An area setting unit;
A corresponding area having a correlation with each of the reference areas is set on an image plane defined by the other image data of the first frame or the second frame, and a position of the corresponding area on the image plane A corresponding area setting unit that specifies a corresponding point indicating
Three reference points are selected from the plurality of reference points specified by the reference area setting unit, and respective three-dimensional positions of the selected reference points are calculated based on the distance data of the one frame. At the same time, among the plurality of corresponding points specified by the corresponding region setting unit, three corresponding points are selected, and three-dimensional positions of the selected corresponding points are determined based on the distance data of the other frame. A three-dimensional position calculator for calculating,
A moving amount of the moving body between the first frame and the second frame based on the three-dimensional position of the reference point and the three-dimensional position of the corresponding point calculated by the three-dimensional position calculating unit; The terrain recognizing device according to any one of claims 1 to 4, further comprising the movement amount detection unit configured to detect the movement amount. The method according to claim 5, wherein the three-dimensional position calculation unit calculates a three-dimensional position of the selected reference point after evaluating a positional relationship between the selected reference points. Terrain recognition device. In a terrain recognition method that is mounted on a moving object and recognizes the three-dimensional shape of the ground,
By a stereo camera, the scene including the ground is captured in time series, and a pair of image data is output for each frame,
Based on the pair of image data, a parallax is calculated by stereo matching, and distance data in which the calculated parallax is associated with a position on an image plane defined by the image data is output for each frame. Steps and
Generating terrain data indicating a three-dimensional shape of the terrain in the first frame based on the distance data of the first frame;
Based on the image data of the first frame, the image data of the second frame preceding the first frame, the distance data of the first frame, and the distance data of the second frame. Detecting the amount of movement of the moving body between the first frame and the second frame;
Generating a terrain map by sequentially combining the terrain data generated for each frame in consideration of the detected movement amount. First and second frame image data obtained by chronologically capturing a scene including the ground with a stereo camera, and a first frame obtained by performing a stereo matching process on the first frame image data. Based on distance data of a frame and distance data of a second frame obtained by performing a stereo matching process on image data of the second frame before the first frame, a moving amount of the moving body is calculated. In the moving amount detection method to be detected,
Setting a plurality of reference areas on an image plane defined by image data of one of the first frame and the second frame, and specifying a reference point indicating a position of the reference area on the image plane When,
A corresponding area having a correlation with each of the reference areas is set on an image plane defined by the other image data of the first frame or the second frame, and a position of the corresponding area on the image plane Identifying a corresponding point indicating
Among the plurality of specified reference points, three reference points are selected, and the respective three-dimensional positions of the selected reference points are calculated based on the distance data of the one frame, and the specified Selecting three corresponding points from among the plurality of corresponding points, and calculating a three-dimensional position of each of the selected corresponding points based on the distance data of the other frame;
Detecting a movement amount of a moving body between the first frame and the second frame based on a three-dimensional position of the reference point and a three-dimensional position of the corresponding point. The moving amount detection method of the moving body to be described.

Images