特許 6556675 物体検出方法及びその装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6556675

(24)【登録日】2019年7月19日

(45)【発行日】2019年8月7日

(54)【発明の名称】物体検出方法及びその装置

(51)【国際特許分類】

   G01B 11/00 20060101AFI20190729BHJP

   G01C 3/06 20060101ALI20190729BHJP

【ＦＩ】

   G01B11/00 H

   G01C3/06 110V

【請求項の数】14

【全頁数】28

(21)【出願番号】特願2016-166341(P2016-166341)

(22)【出願日】2016年8月26日

(65)【公開番号】特開2018-31753(P2018-31753A)

(43)【公開日】2018年3月1日

【審査請求日】2018年10月4日

(73)【特許権者】

【識別番号】501055145

【氏名又は名称】株式会社ＺＭＰ

(74)【代理人】

【識別番号】100082876

【弁理士】

【氏名又は名称】平山 一幸

(72)【発明者】

【氏名】渡邉 龍人

(72)【発明者】

【氏名】サルブッチ ヴァレリオ

【審査官】 齋藤 卓司

(56)【参考文献】

【文献】 特開２０１４−１２００２６（ＪＰ，Ａ）

【文献】 特開２０１５−０３２１８５（ＪＰ，Ａ）

【文献】 特開２００４−１７８２２９（ＪＰ，Ａ）

【文献】 特開２０１６−０９９９３２（ＪＰ，Ａ）

【文献】 特表２０１６−５２２５０８（ＪＰ，Ａ）

【文献】 米国特許出願公開第２０１３／００６０５４０（ＵＳ，Ａ１）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｂ １１／００

Ｇ０１Ｃ ３／０６

(57)【特許請求の範囲】

【請求項1】

ステレオカメラで撮影した物体の左右画像から画素毎に視差を求め、３次元点群データからなる３次元距離画像を生成し、前記３次元点群データを、グリッド状の複数のセルが２次元面に配置された２次元マップに投票して第１占有グリッドマップを生成し、前記第１占有グリッドマップを用いて前記物体の３次元情報を検出する物体検出方法において、
前記複数のセルの境界付近に跨って、冗長なセルをオーバーラップさせて配置し、前記冗長なセルにも、前記３次元点群データを投票して、冗長な第２占有グリッドマップを生成し、前記第２占有グリッドマップを用いて前記物体の３次元情報を検出することを特徴とする、物体検出方法。

【請求項2】

前記２次元マップに投票する際に、
前記物体までの距離に応じた重みを掛けることを特徴とする、請求項１記載の物体検出方法。

【請求項3】

前記重みは、前記物体までの距離の二乗であることを特徴とする、請求項２記載の物体検出方法。

【請求項4】

前記第２占有グリッドマップに対する投票値を、閾値を用いて２値化して２値のマップを生成し、前記２値のマップに対してラベリング処理を行うことを特徴とする、請求項１〜３のいずれか１項記載の物体検出方法。

【請求項5】

前記ラベリング処理の前に収縮処理及び膨張処理を行うことを特徴とする、請求項４記載の物体検出方法。

【請求項6】

前記物体までの距離に応じて、投票する前記セルの範囲を広げることを特徴とする、請求項１〜５のいずれか１項記載の物体検出方法。

【請求項7】

前方路面のパラメータを前記投票の前に前記３次元点群データを用いて推定し、前記３次元点群データを、前記パラメータを用いて路面座標系に変換することを特徴とする、請求項１〜６のいずれか１項記載の物体検出方法。

【請求項8】

物体を撮影して前記物体の左右画像を取得するステレオカメラと、
前記左右画像から画素毎に視差を算出する視差算出部と、
前記視差から、３次元点群データからなる３次元距離画像を生成する３次元距離画像生成部と、
前記３次元点群データを、グリッド状の複数のセルが２次元面に配置された２次元マップに投票して、第１占有グリッドマップを生成する占有グリッドマップ生成部と、
前記第１占有グリッドマップを用いて前記物体の３次元情報を検出する物体検出部と、
を有し、
前記占有グリッドマップ生成部は、
前記複数のセルの境界付近に跨って、冗長なセルをオーバーラップさせて配置し、前記冗長なセルにも、前記３次元点群データを投票して、冗長な第２占有グリッドマップを生成し、
前記物体検出部は、
前記第２占有グリッドマップを用いて前記物体の３次元情報を検出することを特徴とする、物体検出装置。

【請求項9】

前記占有グリッドマップ生成部は、
前記２次元マップに投票する際に、前記物体までの距離に応じた重みを掛けることを特徴とする、請求項８記載の物体検出装置。

【請求項10】

前記重みは、前記物体までの距離の二乗であることを特徴とする、請求項９記載の物体検出装置。

【請求項11】

前記物体検出部は、
前記第２占有グリッドマップに対する投票値を、閾値を用いて２値化して２値のマップを生成し、前記２値のマップに対してラベリング処理を行うことを特徴とする、請求項８〜１０のいずれか１項記載の物体検出装置。

【請求項12】

前記物体検出部は、
前記ラベリング処理の前に収縮処理及び前記膨張処理を行うことを特徴とする、請求項１１記載の物体検出装置。

【請求項13】

前記占有グリッドマップ生成部は、
前記物体までの距離に応じて、投票する前記セルの範囲を広げることを特徴とする、請求項８〜１２のいずれか１項記載の物体検出装置。

【請求項14】

前方路面のパラメータを前記投票の前に前記３次元点群データを用いて推定し、前記３次元点群データを、前記パラメータを用いて路面座標系に変換することを特徴とする、請求項８〜１３のいずれか１項記載の物体検出装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、占有グリッドマップ（Occupancy Grid Map、以下「ＯＧＭ」という。）を用いて、視点から物体までの距離、高さ等の３次元（３Ｄ）情報を検出する物体検出方法及びその装置に関するものである。

【背景技術】

【0002】

図２７は、従来の物体検出方法を示す概略図である。従来の物体検出方法では、立体的な物体の画像を取得する一対のセンサ、例えば、左カメラ１０１Ｌ及び右カメラ１０１Ｒからなるステレオカメラ１００を用いている。ステレオカメラ１００によって取得された左画像及び右画像からなるステレオ画像から、３Ｄ距離画像生成処理部１２０により視差が求められ、３次元点群データ（即ち、３Ｄデータ）からなる３Ｄ距離画像が生成される。そして、予め定めた傾きによる座標変換部１２５とＯＧＭ生成処理部１３０により、３Ｄ距離画像から地表が推定され、指定された高さ以上の物体が抽出されて、真上から見下ろされた２次元（２Ｄ）マップの対応する位置に表示された２ＤのＯＧＭ１０が生成される。

【0003】

図２８は、図２７の従来のＯＧＭ１０の例を示す図である。ＯＧＭ１０は、ステレオカメラ１００の位置を原点にして、左右方向をＸ軸、前後の奥行き方向をＹ軸とした絶対座標系を、基準単位で格子状（グリッド状）の複数のセル１１に分割し、前方の立体的な物体の占有確率（存在確率）をセル１１毎に表示したものである。セル１１上には、物体を検知して投票した点群１３ａが表示されている。なお、セルサイズは、任意あるいはステレオカメラ１の検出精度（解像度）に応じて最適なサイズに決定される。

【0004】

特許文献１には、ＯＧＭを使用して物体、例えば、走行する車両の周囲の物体を検出する物体検出方法が記載されている。特許文献１の車両用ＯＧＭ１０では、一対のセンサが搭載された車両の走行状況に適合するように、車両の近接領域ではセル１１の面積が小さく、車両の遠方領域ではセル１１の面積が大きく構成され、あるいは、車両の速度が速い場合にはセル１１の面積が大きく、車両の速度が遅い場合にはセル１１の面積が小さく構成されるようになっている。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】特表２０１６−５２２５０８号公報

【発明の概要】

【発明が解決しようとする課題】

【0006】

従来のＯＧＭ１０を用いた物体検出方法あるいは物体検出装置では、以下の（ａ）〜（ｅ）のような課題１〜５があった。

【0007】

（ａ） 課題１
従来のＯＧＭ１０では、一対のセンサが検出した位置に対応するグリッド内のセル１１に投票していくが、その位置によって、有効となる投票数が変化してしまい、物体の検出が不安定になることがある。つまり、従来のＯＧＭ１０では、物体が存在する部分に対応するセル１１内の投票数（即ち、点群１３ａの数）が閾値以上であれば、そこに物体が存在するとしていたが、同一物体が複数のセル１１に跨っている場合がある。仮に２つのセル１１，１１の境界線上に物体があったとすると、その物体は、２つのセル１１，１１に分かれて投票されることになり、もしその物体が検出できるぎりぎりの閾値を設定していた場合には、その物体が検出されないことになる。

【0008】

（ｂ） 課題２
課題２の１として、ステレオカメラ１００のノイズの塊によって、本来存在しない物体がセル１１となって検出される問題がある。
また、課題２の２として、従来のＯＧＭ１０では、一つの物体に対して複数のセル１１が対応する場合がある。この場合、ＯＧＭ１０をその後の認識や制御で使う時には、複数に渡るセル１１を統合して一つの物体として扱えるようにすることが望ましいが、そのようにできなかった。

【0009】

（ｃ） 課題３
一対のセンサとして、例えば、ステレオカメラ１００を用いた場合、検出される物体は、そのステレオカメラ１００に面した物体の面積に応じて投票数が決まる。この際、同じ物体であっても、近くにある場合は、ステレオカメラ１００の画素に対する投影面積が大きくなるので、その物体の見えている部分の面積に対する投票数が多くなる。一方、物体が遠方にある場合には、その物体の同じ面積に対応するステレオカメラ１００の画素数が小さくなるので、投票数が少なくなってしまう。そのため、遠くの物体ほど検出し難いという欠点がある。

【0010】

（ｄ） 課題４
一対のセンサとして、例えば、ステレオカメラ１を用いた場合、遠方になると、ステレオカメラ１００による測距精度が低下する。そのため、近場では、物体に対する投票（即ち、物体の点群１３ａ）が一つのセル１１内に収まっていたものが、遠方になると、ステレオカメラ１００の測距精度が悪化するので、二つ以上のセル１１に分散されてしまい、一つ当たりのセル１１への投票数が少なくなってしまい、遠方の物体ほど検出し難いという欠点がある。

【0011】

（ｅ） 課題５
一般に、ＯＧＭを作成する前に３次元点群データを作成する。この際、路面に対して傾いているカメラ座標系から、Ｘ−Ｙ座標軸の作る平面が前方の路面と一致するように新たに取得した路面座標系へと変換する必要がある。この際、従来は、車両等に取り付けたカメラに対してキャリブレーションを行ったり、あるいは画面によって、カメラの前方路面に対するカメラの向きの傾きと高さをあらかじめ決めて、その値を用いて座標変換する必要があった。しかし実際には、車両の後部に重い荷物を積んだり、あるいは後部座席に人が乗ったり乗らなかったり、あるいは急な加減速によって、車両のピッチ角が変動するなどして、実際の路面に対して常に正しい路面座標系に変換されているとは限らなかった。

【0012】

本発明では、より精度の高い３Ｄ情報が得られる物体検出方法を提供することを第１の目的とし、物体検出装置を提供することを第２の目的とする。

【課題を解決するための手段】

【0013】

第１の目的を達成するために、本発明の物体検出方法は、ステレオカメラで撮影した物体の左右画像から画素毎に視差を求め、３Ｄ点群データからなる３Ｄ距離画像を生成し、前記３Ｄ点群データを、グリッド状の複数のセルが２次元面に配置された２Ｄマップに投票して、第１ＯＧＭを生成し、前記第１ＯＧＭを用いて前記物体の３Ｄ情報を検出する方法であって、前記複数のセルの境界付近に跨って、冗長なセルをオーバーラップさせて配置し、前記冗長なセルにも、前記３Ｄ点群データを投票して、冗長な第２ＯＧＭを生成し、前記第２ＯＧＭを用いて前記物体の３Ｄ情報を検出することを特徴とする。

【0014】

第２の目的を達成するために、本発明の物体検出装置は、物体を撮影して前記物体の左右画像を取得するステレオカメラと、前記左右画像から画素毎に視差を算出する視差算出部と、前記視差から、３Ｄ点群データからなる３Ｄ距離画像を生成する３Ｄ距離画像生成部と、前記３Ｄ点群データを、グリッド状の複数のセルが２次元面に配置された２Ｄマップに投票して、第１ＯＧＭを生成するＯＧＭ生成部と、前記第１ＯＧＭを用いて前記物体の３Ｄ情報を検出する物体検出部と、を有している。
そして、前記ＯＧＭ生成部は、前記複数のセルの境界付近に跨って、冗長なセルをオーバーラップさせて配置し、前記冗長なセルにも、前記３Ｄ点群データを投票して、冗長な第２ＯＧＭを生成し、前記物体検出部は、前記第２ＯＧＭを用いて前記物体の３Ｄ情報を検出することを特徴とする。

【0015】

前記発明の物体検出方法及びその装置において、例えば、前記２Ｄマップに投票する際に、前記物体までの距離に応じた重みを掛けてもよい。
前記第２ＯＧＭに対する投票値を、閾値を用いて２値化して２値のマップを生成し、前記２値のマップに対してラベリング処理を行ってもよい。前記ラベリング処理の前に収縮処理及び膨張処理を行ってもよい。また、前記物体までの距離に応じて、投票する前記セルの範囲を広げてもよい。
さらに、前方路面のパラメータを前記投票の前に前記３次元点群データを用いて推定し、前記３次元点群データを、前記パラメータを用いて路面座標系に変換してもよい。

【発明の効果】

【0016】

本発明の物体検出方法及びその装置によれば、複数のセルの境界付近に跨って、冗長なセルをオーバーラップさせて配置し、その冗長なセルにも、３Ｄ点群データを投票しているので、同一物体が複数のセルに跨っている場合であっても、その物体を的確に検出できる。

【0017】

２Ｄマップに投票する際に、物体までの距離に応じた重みを掛けた場合には、遠くの物体ほど検出し難いという課題を解決できる。
２値のマップに対して収縮処理及び膨張処理を行うことにより、ステレオカメラの小さなノイズを的確に除去できる。
ラベリング処理を行うことにより、複数に渡るセルを統合して一つの物体として扱えるようになり、第２ＯＧＭを用いた物体の認識や制御が容易になる。
また、物体までの距離に応じて、投票するセルの範囲を広げれば、遠方の物体ほど検出し難いという課題を解決できる。

【図面の簡単な説明】

【0018】

【図1】本発明の実施例１における物体検出装置の全体を示す概略の機能ブロック図である。

【図2A】本実施例１の物体検出方法の全体の処理を示すフローチャートである。

【図2B】図２ＡのステップＳ４の詳細を示すフローチャートである。

【図3A】本実施例１におけるＲＯＧＭの例を示す図である。

【図3B】従来のＯＧＭの例を示す図である。

【図4A】本実施例１におけるＲＯＧＭの他の例を示す図である。

【図4B】従来のＯＧＭの他の例を示す図である。

【図5】本実施例１におけるＲＯＧＭの他の例を示す図である。

【図6A】本実施例１のＲＯＧＭにおける複数のセルの配置例を示す図である。

【図6B】本実施例１のＲＯＧＭにおける複数のセルの配置例を示す図である。

【図6C】本実施例１のＲＯＧＭにおける複数のセルの配置例を示す図である。

【図6D】従来のＯＧＭにおける複数のセルの配置例を示す図である。

【図7A】本実施例１におけるＲＯＧＭのアルゴリズム１の処理例を示すフローチャートである。

【図7B】本実施例１におけるＲＯＧＭのアルゴリズム２の処理例を示すフローチャートである。

【図8】課題３の解決方法を説明するための図である。

【図9】図２ＡのステップＳ９における収縮処理を示す図である。

【図10】図２ＡのステップＳ９における膨張処理を示す図である。

【図11A】ラベリング処理において対象とする２値化画像を示す図である。

【図11B】ラベリング処理において４連結の場合を示す図である。

【図11C】ラベリング処理において８連結の場合を示す図である。

【図11D】ラベリング処理においてラスタスキャンとルックアップテーブルを示す図である。

【図11E】ラベリング処理において白い画素に対するラベル番号の振り方を示す図である。

【図11F】ラベリング処理において最初に番号を割り振った様子を示す図である。

【図11G】ラベリング処理において更にラスタスキャンを続ける様子を示す図である。

【図11H】ラベリング処理において新たな番号（２）を割り振った様子を示す図である。

【図11I】ラベリング処理において更に新たな番号（３）を割り振った様子を示す図である。

【図11J】ラベリング処理においてルックアップテーブルの書き換えの様子を示す図である。

【図11K】ラベリング処理において更にルックアップテーブルの書き換えを示す図である。

【図11L】ラベリング処理において全ての画素に対してラベル番号が割り振られた状態を示す図である。

【図11M】ラベリング処理においてラベル番号が修正された状態を示す図である。

【図12】（ａ）〜（ｃ）は、ステレオカメラの位置を横方向で変えたときの実施例の画像を示す図である。

【図13】実施例のＲＯＧＭによる箱の検出の投票数を示す図である。

【図14】比較例のＯＧＭによる箱の検出を示し、（ａ）は横方向の投票数を、（ｂ）は横方向及び奥行き（距離）方向の投票数を示す図である。

【図15】ノイズを調べるのに用いた画像を示し、（ａ）が右カメラの画像を、（ｂ）が距離画像を示す図である。

【図16】距離画像から得られる３Ｄ点群に対して、ＲＯＧＭ処理をした投票結果を示す図である。

【図17】図１６のデータを閾値が１００００として２値化した結果を示す図である。

【図18】図１７のデータを収縮・膨張処理した結果を示す図である。

【図19】図１８の２値データをラベリング処理した結果を示す図である。

【図20】遠方の投票を調べるのに用いた画像を示し、（ａ）が左画像を、（ｂ）が右画像を、（ｃ）は距離画像である。

【図21】実施例のＲＯＧＭによるマップである。

【図22】比較例のＯＧＭによるマップである。

【図23】遠方の車両を調べるのに用いた画像を示し、（ａ）が左画像を、（ｂ）は距離画像を示す図である。

【図24】実施例の３Ｄ値から求めたＲＯＧＭの結果を示す図である。

【図25】図２４のデータを２値化した結果を示す図である。

【図26】図２５のデータを収縮・膨張処理した結果を示す図である。

【図27】従来の物体検出方法を示す概略の図である。

【図28】図２７の従来のＯＧＭの例を示す図である。

【発明を実施するための形態】

【0019】

本発明を実施するための形態は、以下の好ましい実施例の説明を添付図面と照らし合わせて読むと、明らかになるであろう。但し、図面はもっぱら解説のためのものであって、本発明の範囲を限定するものではない。

【実施例1】

【0020】

（実施例１の物体検出装置の全体の構成）
図１は、本発明の実施例１における物体検出装置２０の全体を示す概略の機能ブロック図である。

【0021】

物体検出装置２０は、物体を撮影する左カメラ２１Ｌ及び右カメラ２１Ｒからなるステレオカメラ２１を有している。このステレオカメラ２１の出力側には、視差算出部２２、３Ｄ距離画像生成部としての距離・３Ｄ情報算出部２３、前方路面からのカメラ高さ・傾き推定部２４、前方路面に合わせた傾き変換部２５、ＯＧＭ生成部としてのＯＧＭ算出部２６、ノイズ除去部２７、物体検出部２８、検出物体情報の出力部２９及び、グローバルマップ表示部３０が、縦続接続（cascade connection）されている。これらの視差算出部２２、距離・３Ｄ情報算出部２３、前方路面からのカメラ高さ・傾き推定部２４、前方路面に合わせた傾き変換部２５、ＯＧＭ算出部２６２６、ノイズ除去部２７、物体検出部２８、検出物体情報の出力部２９及びグローバルマップ表示部３０は、例えばコンピュータ等の情報処理装置によって構成されている。

【0022】

ステレオカメラ２１は、左カメラ２１Ｌによって左画像を取得し、右カメラ２１Ｒによって右画像を取得する装置であり、この出力側に視差算出部２２が接続されている。視差算出部２２は、ステレオカメラ２１から入力される左右画像から画素毎の視差を求めるものであり、この出力側に距離・３Ｄ情報算出部２３が接続されている。

【0023】

距離・３Ｄ情報算出部２３は、算出された視差から、物体までの距離と３Ｄ座標値を求めるものであり、この出力側に、前方路面からのカメラ高さ・傾き推定部２４及び前方路面に合わせた傾き変換部２５が接続されている。前方路面からのカメラ高さ・傾き推定部２４及び前方路面に合わせた傾き変換部２５は、高さ・ピッチ・ロール変換を行うものであり、前方の路面から平面の傾き（ピッチ、ロール）と平面に対するカメラ位置高さとを推定し、この推定した高さ・傾き情報から、３Ｄ座標値を路面に平行な３Ｄ座標系へと変換する機能を有し、この出力側に、ＯＧＭ算出部２６が接続されている。

【0024】

ＯＧＭ算出部２６は、第１ＯＧＭへの投票・閾値処理を行うものであり、変換された３Ｄ座標系において、ある条件を満たす３Ｄ点を路面に平行な２Ｄマップである第１ＯＧＭ上に、予め決めた重みを掛けて投票し、得られた投票数から、第２占有グリッドマップ（Redundant Occupancy Grid Map、以下「ＲＯＧＭ」又は「第２ＯＧＭ」という。）に対する投票値を求める。ＲＯＧＭとは、従来のＯＧＭを改良し、物体位置に関わらず、安定した物体検出を可能にする２次元マップであり、従来のＯＧＭに対し、「冗長な」という意味のRedundantを加えた略語である。ＯＧＭ算出部２６では、更に、得られたＲＯＧＭの投票値に対して、画像を２値化する機能を有している。このＯＧＭ算出部２６の出力側には、ノイズ除去部２７と物体検出部２８が接続されている。

【0025】

ノイズ除去部２７と物体検出部２８は、算出された２値のマップであるＲＯＧＭに対して適切な回数の収縮・膨張処理を行い、処理された２値のＲＯＧＭに対してラベリング処理を行い、更に、予め決めた幅、高さ等の大きさを満たす物体のみを抽出する機能を有し、この出力側に、検出物体情報の出力部２９が接続されている。ここで、収縮・膨張処理はノイズ除去部２７により、ラベリング処理は、物体検出部２８により行われる。

【0026】

検出物体情報の出力部２９は、抽出された物体に識別子（ＩＤ）、幅、高さ等の情報を付与して出力するものであり、この出力側に、グローバルマップ表示部３０が接続されている。グローバルマップ表示部３０は、出力部２９の出力結果を表示画面に表示するものである。

【0027】

（実施例１の物体検出方法の全体の処理）
図２Ａは、図１の物体検出装置２０を用いた本実施例１の物体検出方法における全体の処理を示すフローチャートであり、図２Ｂは、図２ＡのステップＳ４の詳細を示すフローチャートである。

【0028】

本実施例１の物体検出方法では、図１の物体検出装置２０により、ステップＳ１〜Ｓ１２の処理が行われる。

【0029】

物体検出の処理が開始されると、先ず、ステップＳ１において、ステレオカメラ２１によって前方の物体が撮影され、撮影された左右の画像が、視差算出部２２へ入力され、ステップＳ２へ進む。ステップＳ２において、視差算出部２２は、入力された左右画像から画素毎に視差値を求め、ステップＳ３へ進む。ステップＳ３において、距離・３Ｄ情報算出部２３は、求められた視差値から物体までの距離と３Ｄ座標値を求め、ステップＳ４，Ｓ５へ進む。

【0030】

ステップＳ４において、前方路面からのカメラ高さ・傾き推定部２４及び前方路面に合わせた傾き変換部２５は、前方の路面から平面の傾き（ピッチ、ロール）と平面に対するカメラ位置高さとを推定し、ステップＳ５へ進む。ステップＳ５において、傾き変換部２４は、推定した高さ・傾き情報から、３Ｄ座標値を路面に平行な３Ｄ座標系へと変換し、ステップＳ６，Ｓ７，Ｓ８へ進む。

【0031】

ステップＳ４の詳細を図２Ｂにより説明する。
ステップＳ４−１において、前方路面の指定された範囲の３Ｄ点座標のpi＝ (Xi, Yi, Zi)の個数を求めnとし、全てのpiに対する平均値を取り、重心qとし、ステップＳ４−２において、分散共分散行列Ｃを求める。
ステップＳ４−３において、ヤコビ法によりＣの固有値と固有ベクトルを求め、最小固有値に対応する固有ベクトルを(u_X, u_Y, u_Z)とし、ステップＳ４−４において、全てのpiに対してh＝u_XX+u_YY+u_ZZを求め、hの平均値ε、分散σを求める。
ステップＳ４−５において、ε−2σ＜h＜ε+2σを満たさないpiをサンプル点から除外する。
ステップＳ４−６において、残ったサンプル点に対して前方路面の指定された範囲の３Ｄ点座標のpi＝ (Xi, Yi, Zi)の個数を求めn とし、ステップＳ４−７において、全てのpiに対する平均値を取り、重心q＝(Xa, Ya, Za)とし、分散共分散行列C＝Σ(pi−q)(pi−q)^Tを求める。
ステップＳ４−８において、ヤコビ法により、Ｃの固有値と固有ベクトルを求め、最小固有値に対応する固有ベクトルを(uX, uY, uZ)とし、ステップＳ４−９において、ロール(θ_X), ピッチ(θ_Y), ヨー(θ_Z）を求め、ステップＳ４−１０においてカメラの高さ、ｈ（h＝u_XXa+u_YYa+u_ZZa）を求める。

【0032】

上記のステップＳ４の各ステップによる、所謂平面当てはめ法によりカメラの高さの推定ができる。つまり、ステレオカメラ２１で得られた前方路面のある範囲内の３次元点群データを用いて、前方路面を近似する平面の法線ベクトルを求め、その法線ベクトルからカメラ座標のロール、ピッチ、ヨーの３つの角度が求まり、その法線ベクトルから、その平面の高さのパラメータも求まり、カメラの路面に対する高さが求まる。従って、前方路面の傾き等を反映した路面座標（Ｘｓ，Ｙｓ，Ｚｓ）が得られる。本発明では、このように推定した路面座標を、路面座標系とも呼ぶ。

【0033】

ステップＳ４の処理をリアルタイムで実行することにより、リアルタイムでカメラの路面に対する傾き情報と高さ情報が得られる。これにより、後部重量物の搭載や急な加減速による車両の状態の変化による影響を避けることができる。車両に近い前方路面は、一般に平坦であると想定できる。アスファルト等の路面に対して、良好なステレオカメラキャリブレーションを施され、十分な広さのダイナミックレンジを持つカメラであれば、撮影されたカメラ画像は、ステレオマッチングのために十分なテクスチャを持つため、路面上の密な３Ｄ点群情報が得られる。これらの情報から、前方路面を近似するような前方路面の傾き等を反映した路面座標系（Ｘｓ，Ｙｓ，Ｚｓ）を得ることができる。これにより、上記課題５を解決することができる。

【0034】

ステップＳ６において、ＯＧＭ算出部２６は、変換された３Ｄ座標系において、ある条件を満たす３Ｄ点を、路面に平行な２次元マップであるＯＧＭ上に予め決めた重みを掛けて投票し、ステップＳ７へ進む。ステップＳ７において、ＯＧＭ算出部２６は、得られた投票数からＲＯＧＭに対する投票値を求め、ステップＳ８へ進む。ステップＳ８において、ＯＧＭ算出部２６は、得られたＲＯＧＭの投票値に対して、ある閾値以上のセルは、物体が存在する状態（１）とし、それ以外のセルには、物体が存在しない状態（０）として画像を２値化し、ステップＳ９，Ｓ１０，Ｓ１１へ進む。

【0035】

ステップＳ９において、ノイズ除去部２７は、算出された２値のＲＯＧＭに対して適切な回数の収縮処理及び膨張処理を行い、ステップＳ１０へ進む。ステップＳ１０において、物体検出部２８は、処理された２値のＲＯＧＭに対してラベリング処理を行い、ステップＳ１１へ進む。ステップＳ１１において、物体検出部２８は、ラベリング処理結果から、予め決めた幅、高さ等の大きさを満たす物体のみを抽出し、ステップＳ１２へ進む。

【0036】

ステップＳ１２において、検出物体情報の出力部２９は、抽出された物体にＩＤ、幅、高さ等の情報を付与する。付与された情報は、グローバルマップ表示部３０によって表示され、物体検出処理が終了する。

【0037】

（図２ＡのステップＳ１〜Ｓ３の詳細）
ステレオカメラ２１による距離算出は、左カメラ２１Ｌと右カメラ２１Ｒとの視差の違いから、カメラ間距離等を用いて、三角測量の原理によって求められる。同じレンズとセンサを用いたカメラを左カメラ２１Ｌ及び右カメラ２１Ｒとして用い、レンズの焦点距離をｆ、カメラ間距離（ベースライン長）をｗ、センサの画素サイズ（一辺のサイズ）をｕ、そして視差をｄ（ｐｉｘ）とすると、ステレオカメラ２１から物体までの距離Ｌは、次式（１）で表される。
Ｌ＝ｆｗ／ｄｕ・・・・（１）

【0038】

この式（１）から分かるように、距離Ｌは、視差ｄに反比例するため、視差ｄが小さいほど遠くなり、視差ｄが大きいほど近くになる。また、視差ｄの変化に対する距離Ｌの変化を考えると、式（１）から下記の式（２）が求まる。この式（２）は、ある視差ｄにおいて、視差ｄの微小な変化（視差誤差△ｄ）に対して、距離Ｌがどれくらい変わるかを示している。ここで、△Ｌは、距離の変化（距離誤差）を示す。例えば、レンズ焦点距離ｆ＝６ｍｍ、カメラ間距離ｗ＝２１０ｍｍ、センサ画素サイズｕ＝３．７５μｍ（＝０．００３７５ｍｍ）という値を実際に考えた場合の式が得られている。

【0039】

【数1】

【0040】

一般に、ステレオカメラ２１を用いて視差ｄを求める際に、１画素未満の精度で求める手法が提案され、それによって、例えば、１／４〜１／１０画素程度の精度が実現されている。実際にその精度が実現されるかどうかは、被写体となる物体上のテクスチャの濃さやエッジの強さによって影響される。

【0041】

仮に、物体が常にある程度のテクスチャがあると仮定でき、且つ、それによって視差誤差△ｄの大きさのサブピクセル（例えば、１／４ｐｉｘ）であるとすると、ある視差ｄに対して、式（２）で表されるような距離誤差△Ｌが得られる。
本実施例１では、その距離誤差△Ｌを基にしてＯＧＭに対する投票を考え直している。

【0042】

即ち、ステレオ処理（左右画素間の対応づけ）を行って、ある画素に対して視差ｄが求まっている。そして、期待できるサブピクセル精度として、視差誤差△ｄを決めておく。その場合に、式（２）によって、視差ｄに対する距離Ｌの誤差ΔＬを含んだ変動幅が得られる。その範囲を確率密度と考え、その範囲に含まれるセルに対して投票を行う。その際に、使用する情報処理装置の性能に応じて、性能が高ければ、ガウス分布のような分布に応じた投票とし、また、性能が低ければ、同じ確率密度を持つものとして、均等に投票する。これにより従来の課題４を解決している。

【0043】

（図２ＡのステップＳ３〜Ｓ７の詳細）
図３Ａは本発明の実施例１におけるＲＯＧＭ４０の例を示す図であり、図３Ｂは、第１ＯＧＭとしての従来のＯＧＭ１０の例を示す図である。ＯＧＭ１０には、複数のセル１１（＝１１−１，１１−２）が配置されている。なお、各セル１１は正方形として図示されているが、ＯＧＭ１０の設計によって、それは横長の長方形でも縦長の長方形でもなりうるが、ここでは説明の便宜上、正方形としている。

【0044】

図３Ｂに示すように、物体がたまたまＯＧＭ１０の２つのセル１１−１，１１−２の境界位置にあったとき、物体を検知して投票した点群１３ａは、図示のようになる。

【0045】

ＯＧＭ１０の１つの効能として、センサであるステレオカメラ２１のノイズの除去が挙げられる。本当の物体に対するステレオカメラ出力の点群１３ａの数に対して、ステレオカメラ２１のノイズによる点群の数は、相対的に少ないと考えられる。そのため、セル１１−１，１１−２に投票された点群１３ａの数に対して適切な閾値を設定して、それ以上のものだけを物体として検出することにより、ノイズを除去できる。

【0046】

もし、その閾値を、物体検出のために必要な数に対してぎりぎりの値としていた場合、２つのセル１１−１，１１−２の境界位置に物体が存在していると、この物体を検出できないおそれがある。例えば、図３Ｂの場合には、物体に対して１０個の点群１３ａが投票されている。この場合、閾値をぎりぎりの１０個としていると、実際の投票は、左側のセル１１−１と右側のセル１１−２に分かれているので、どちらのセル１１−１，１１−２も５個の投票となり、閾値の１０個に届かず、その物体を検出できない。そこで、本実施例１では、そのような従来の課題１を解決するために、図３Ａに示すような第２ＯＧＭとしてのＲＯＧＭ４０を用いている。

【0047】

図３Ａに示すように、ＲＯＧＭ４０には複数のセル３１（例えば、２つのセル３１−１，３１−２）が配置され、これらの２つのセル３１−１，３１−２の中央に、新たに領域をオーバーラップさせた冗長なセル３１−３が配置されている。中央の冗長なセル３１−３にも、左右のセル３１−１，３１−２と同様に投票すると、その冗長なセル３１−３に対する投票された点群３３ａの数が１０個となり、閾値以上の投票となって従来のＯＧＭ１０では検出できなかった物体を検出できる。

【0048】

つまり、従来のＯＧＭ１０では、格子状のオーバーラップしないセル１１−１，１１−２を用いているが、本実施例１では、オーバーラップさせたセル３１−３を冗長に配置することで、左右のセル３１−１，３１−２の境界付近に存在する物体（３３ａ）の検出を可能にし、物体位置に関わらず安定した検出を可能にしている。
図４Ａは、本発明の実施例１におけるＲＯＧＭ４０の他の例を示す図であり、図４Ｂは従来のＯＧＭ１０の他の例を示す図である。

【0049】

図３Ｂの場合は、従来のＯＧＭ１０において２つのセル１１−１，１１−２の境界付近に物体が存在した場合であったが、図４Ｂのように、従来のＯＧＭ１０において４つのセル１１（＝１１−１〜１１−４）の境界付近に物体が存在する場合もある。このような場合には、図４Ａに示すように、本実施例１のＲＯＧＭ４０において４つのセル３１（＝３１−１〜３１−４）の中央に、冗長な１つのセル３１−５を配置することによって、図３Ａの２つのセル３１−１，３１−２の場合と同様に、中央に追加したセル３１−５では、投票された点群３３ａの数が１０個となり、その物体が検出できるようになる。

【0050】

以上述べたことを、一般性をもって処理できるように、ＲＯＧＭ４０のアルゴリズムを構成する。

【0051】

図５は、本発明の実施例１におけるＲＯＧＭ４０の他の例を示す図である。本実施例１の図３Ａに示すＲＯＧＭ４０では、１つのセル３１−１に対して横方向に、セル３１−１の幅の半分だけずらして、新たなセル３１−３を配置する。同様に、本実施例１の図５に示すＲＯＧＭ４０では、縦方向に、セル３１−１の高さの半分だけずらして、新たなセル３１−５を配置する。そして、本実施例１の図４Ａに示すように、斜め方向に新たなセル３１−５を配置する。

【0052】

図６Ａ、図６Ｂ及び図６Ｃは、本実施例１のＲＯＧＭ４０における複数のセル３１の別の配置例を示す図であり、図８Ａは、従来のＯＧＭ１０における複数のセル１１の配置例を示す図である。

【0053】

前記の図３Ａ、図３Ｂ、図４Ａ、図４Ｂ、図５の複数のセル３１，１１の配置例から、それらのセル３１、１１の中心を「×」で表すと、本実施例１のＲＯＧＭ４０のセル３１の配置は、図６Ａのようになり、従来のＯＧＭ１０のセル１１の配置は、図６Ｄのようになる。従来のセル１１の中心の数と元々のセル１１の数に対して、密度として比較すると、本実施例１では、横方向で２倍、縦方向で２倍、縦横合計で４倍の数となっている。

【0054】

以上述べたことを、一般性をもって処理できるように、本実施例１のＲＯＧＭ４０のアルゴリズムを以下のように構成していく。

【0055】

図６Ａに示された「×」の点を格子点とするような新たなグリッドを構成する。このグリッドは、元のグリッドの半分（縦横共に）の大きさのセル３１を持っている。但し、新しいセル３１の中心は、「×」印ではなく、セル３１の四隅の位置に「×」印が来るように配置する。その新しいグリッドとセル３１は、図６Ｂのようなものになる。

【0056】

これらのグリッドに対して、従来と同様に、ステレオカメラ２１の出力を、位置に応じて新しい各セル３１に対して投票する。投票が終わってから、縦横２×２の新しいセル３１の全ての投票数を加算して、「×」印の位置の投票数として登録する。これを全ての「×」印に対して行い、その数を登録する。

【0057】

中央の「×」印に対して、投票される新たな小さなセル３１の投票範囲を、図８Ｃの灰色部分３１ｇとして表した。また、元々のセル３１の中央位置は、新しく求めた小さなセルの２×２の領域の中央位置と一致しているので、元のセル３１との位置のずれはない。

【0058】

この処理によって、全ての「×」印の位置において、それに対する投票の範囲は、元のセル３１と同じサイズのものとなり、それが、縦横が元のサイズの半分の割合で求まったことになる。これは、前記のように「×」印の点で、投票範囲がそれぞれ元のセルサイズの半分ずつオーバーラップした範囲の投票数が求まっていることになる。

【0059】

以上で、本実施例１におけるＲＯＧＭ４０の投票処理が終了する。この投票処理により、従来に比して閾値を大きくしても物体の検出が可能となり、よりノイズに強い検出が可能になる。但し、得られた物体位置は、隣のセル３１でも検出されやすくなるので、実際の物体範囲よりも大きめになり、膨らんで検出される。しかし、後で述べる収縮処理及び膨張処理によって、検出されたセル３１を統合し、その統合された複数のセル３１の中から、実際にステレオカメラ２１が出力している部分だけを抽出することにより、膨らんでいない実体サイズに近い領域を検出することができる。従って、従来の課題１を解決できる。

【0060】

図７Ａは、本発明の実施例１におけるＲＯＧＭ４０のアルゴリズム１の処理例を示すフローチャートである。このアルゴリズム１では、図２ＡのステップＳ３〜Ｓ７に対応する処理がステップＳ２０〜Ｓ３６により行われる。

【0061】

処理開始が開始されてステップＳ２０に進むと、ステレオカメラ２１にて取得される左右の画像中の画素（ｉｘ，ｉｙ）（但し、ｉｘ；左右のＸ軸方向の画素値、ｉｙ；前後のＹ軸方向の画素値）において、画素値ｉｙを初期値（＝０）に設定し、更に、次のステップＳ２１において、画素値ｉｘを初期値（＝０）に設定し、ステップＳ２２へ進む。

【0062】

ステップＳ２２において、距離・３Ｄ情報算出部２３は、画素（ｉｘ，ｉｙ）に対するカメラ座標系の３Ｄ座標（Ｘ，Ｙ，Ｚ）を求め、ステップＳ２３へ進む。ステップＳ２３において、前方路面からのカメラ高さ・傾き推定部２４及び前方路面に合わせた傾き変換部２５は、予め決めたステレオカメラ２１に対するロール・ピッチ・高さ情報から、３Ｄ座標（Ｘ，Ｙ，Ｚ）を路面座標（Ｘｓ，Ｙｓ，Ｚｓ）に変換し、ステップＳ２４へ進む。

【0063】

ステップＳ２４において、ＯＧＭ算出部２６は、路面座標（Ｘｓ，Ｙｓ，Ｚｓ）がＯＧＭ１０の範囲内にあるか否かを判定し、範囲外（Ｎｏ）のときにはステップＳ２６へ進み、範囲内（Ｙｅｓ）のときにはステップＳ２５へ進む。ステップＳ２５において、ＯＧＭ算出部２６は、路面座標値Ｘｓ，Ｙｓを整数化した座標値ｉＸｓ，ｉＹｓに対応するＯＧＭ１０に座標値Ｘｓの２乗値を加算し、ステップＳ２６へ進む。

【0064】

なお、座標（Ｘｓ，Ｙｓ）のセルに対する添え字（ｉＸｓ，ｉＹｓ）の密度は、ＲＯＧＭ４０の添え字（ｉＸｓ２，ｉＹｓ２）の密度と同じになる。
ＲＯＧＭ４０の一つのセルのサイズは、後述する式（３）により倍となるため、従来のＯＧＭ１０に比べるとセルサイズは従来のＯＧＭ１０の半分のサイズにしておくことが望ましい。

【0065】

ステップＳ２６において、ＯＧＭ算出部２６は、座標値ｉｘに１を加算し、この加算値ｉｘ＋１が座標値ｎｘより小さいか否か（ｉｘ＜ｎｘ？）をステップＳ２７で判定し、小さいときには（Ｙｅｓ）ステップＳ２２へ戻り、大きいときには（Ｎｏ）ステップＳ２８へ進む。ステップＳ２８において、ＯＧＭ算出部２６は、座標値ｉｙに１を加算し、この加算値ｉｙ＋１が座標値ｎｙより小さいか否か（ｉｙ＜ｎｙ？）をステップＳ２９で判定し、小さいときには（Ｙｅｓ）ステップＳ２１へ戻り、大きいときには（Ｎｏ）ステップＳ３０へ進む。

【0066】

ステップＳ３０において、ＯＧＭ算出部２６は、座標値ｉＹｓ２を初期値（＝０）に設定し、更に、ステップＳ３１において、座標値ｉＸｓ２を初期値（＝０）に設定し、ステップＳ３２へ進む。
ステップＳ３２において、ＯＧＭ算出部２６は、次式（３）の演算を行い、ＲＯＧＭ４０に対する投票値ＲＯＧＭ（ｉＸｓ２，ｉＹｓ２）を求め、更に、ステップＳ３３〜Ｓ３６の処理を繰り返す。
ＲＯＧＭ（ｉＸｓ２，ｉＹｓ２）＝ＯＧＭ（ｉＸｓ２，ｉＹｓ２）＋ＯＧＭ（ｉＸｓ２，ｉＹｓ２＋１）＋ＯＧＭ（ｉＸｓ２＋１，ｉＹｓ２）＋ＯＧＭ（ｉＸｓ２＋１，ｉＹｓ２＋１） ・・・・（３）

【0067】

ＯＧＭ算出部２６は、ステップＳ３３において、座標値ｉＸｓ２に１を加算し、ステップＳ３４において、その加算値ｉＸｓ２＋１が座標値ｎＸｓより小さいか否か（ｉＸｓ２＜ｎＸｓ？）を判定し、小さいときには（Ｙｅｓ）ステップＳ３２へ戻り、大きいときには（Ｎｏ）ステップＳ３５へ進む。更に、ＯＧＭ算出部２６は、ステップＳ３５において、座標値ｉＹｓ２に１を加算し、ステップＳ３６において、その加算値ｉＹｓ２＋１が座標値ｎＹｓより小さいか否か（ｉＹｓ２＜ｎＹｓ？）を判定し、小さいときには（Ｙｅｓ）ステップＳ３１へ戻り、大きいときには（Ｎｏ）アルゴリズム１の処理を終了する。

【0068】

図７Ｂは、本発明の実施例１におけるＲＯＧＭ４０のアルゴリズム２の処理例を示すフローチャートである。このアルゴリズム２では、図２ＡのステップＳ８に対応する処理がステップＳ４０〜Ｓ４５により行われる。

【0069】

ステップＳ４０では、ＯＧＭ算出部２６により、ＲＯＧＭ４０の各セル３１に対する投票値ＲＯＧＭ（ｉＸｓ２，ｉＹｓ２）がアルゴリズム１で求められている。ステップＳ４１において、ＯＧＭ算出部２６は、求められた投票値ＲＯＧＭ（ｉＸｓ２，ｉＹｓ２）が予め決めた閾値ＴＨ以上か否か（投票値ＲＯＧＭ（ｉＸｓ２，ｉＹｓ２）≧閾値ＴＨ？）を判定し、閾値ＴＨ以上のときには（Ｙｅｓ）ステップＳ４２において、そのセル３１に物体が存在し、閾値ＴＨより小さければ（Ｎｏ）、ステップＳ４３において、そのセル３１に物体が存在しないとする。ＯＧＭ算出部２６は、ステップＳ４４において、セル３１に対する物体の存在の状態値として、＋１か０を与える。これにより、ＯＧＭ算出部２６は、ステップＳ４５において、ＲＯＧＭ４０の状態を２値の画像として表示することができる。

【0070】

（図２ＡのステップＳ６の詳細）
図８は、課題３の解決方法を説明するための図である。

【0071】

例えば、ステレオカメラ２１から見て前方に、横２ｍ×縦２ｍのサイズ（前面投影面積）の物体があったとする。画素サイズが５μｍのステレオカメラ２１で、焦点距離が１０ｍｍのレンズを使ったレンズ・カメラ系があった場合、１０ｍでは、２ｍ×２ｍの物体は、次式（４）のような三角形の相似計算により、ステレオカメラ２１上では、４００×４００画素の大きさになる。
２／１０ ＝ ｘ／１０・・・・（４）
ｘ＝２ｍｍ
２／０．００５＝４００画素

【0072】

その物体が４０ｍ遠方にあった場合には、同様に、次式（５）のような三角形の相似計算により、ステレオカメラ２１上では、１００×１００画素の大きさとなる。
２／４０＝ｘ／１０・・・・（５）
ｘ＝２０／４０＝０．５ｍｍ
０．５／０．００５＝１００画素

【0073】

ＯＧＭ１０では、画素１つに対して１つの投票を行うため、この状況では、１０ｍ先の場合には、４００×４００＝１６０，０００点の投票が行われる一方で、４０ｍ先では、１００×１００＝１０，０００点の投票が行われる。ＯＧＭ１０の閾値として同じ値を使うとすると、１０ｍの場合は、４０ｍの場合に対して１６倍も物体が取れやすくなることになる。これを是正するために、本実施例１では、距離に応じて投票に重みをつけることにする。つまり、同じ面積の物体に対して、画素数は距離の二乗に反比例して多くなる。これを補正するためには、物体までの距離の二乗を重みとして、投票数に乗じた値を投票するようにすればよい。これにより従来の課題３を解決できる。

【0074】

例えば、１０ｍ先の場合には１０の二乗の１００を、４０ｍ先の場合には４０の二乗の１６００を掛けた値を１画素毎に投票することにより、１０ｍ先の４００×４００画素の全ての画素による投票数は、１６０，０００×１００＝１６，０００，０００となり、４０ｍ先の１００×１００画素の全ての画素による投票数は、１０，０００×１６００＝１６，０００，０００となり、同じ数となる。この重みは、あくまでも重みなので、実際の距離に対してスケールを掛けた値を用いてもよい。

【0075】

（図２ＡのステップＳ９の詳細）
図９は、図２ＡのステップＳ９における収縮処理を示す図である。さらに、図１０は、図４のステップＳ９における膨張処理を示す図である。この図９及び図１０を参照しつつ、従来の課題２の１の解決方法について説明する。

【0076】

図２ＡのステップＳ９において、ノイズ除去部２７は、算出された２値のＲＯＧＭ４０に対して、適切な回数の収縮処理及び膨張処理を行う。そのために、モルフォロジー演算で使われるオープニング（収縮と膨張）を用いて、複数のセルを一つの塊として抽出して、それを一つの物体として出力する。

【0077】

膨張処理及び収縮処理では、一般的に２値化された白黒の画像に対して処理が行われ、注目画素３５の周辺に１画素でも白い画素があれば白に置き換える処理を膨張（Dilation）といい、逆に周辺に１画素でも黒い画素があれば黒に置き換える処理を収縮（Erosion）という。

【0078】

ノイズにより誤って検出されたセルは、本当の物体に対してサイズが小さいと考えられるので、最初に、図９に示すような収縮処理を行うことで小さなセルが消去され、その後に、図１０に示すような膨張処理を行うことによって元のサイズが復元される。

【0079】

図２ＡのステップＳ１０のラベリング処理を行う前に、オープニング（収縮処理をしてから膨張処理を行う処理）を行うことによって、ステレオカメラ２１の小さなノイズが除去され、大きな構造が残る。ＯＧＭ１０のセル内の投票数の閾値処理によってノイズを除去できるが、更に、オープニングを行うことで、セルとして抽出されてしまったノイズをここで除去することができるので、従来の課題２の１を解決できる。

【0080】

（図２ＡのステップＳ１０の詳細）
図１１Ａ〜図１１Ｍは、従来の課題２の２を解決するための図２ＡのステップＳ１０のラベリング処理を示す図である。

【0081】

図１１Ａ〜図１１Ｍにおいて、図１１Ａは対象とする２値化画像、図１１Ｂは４連結の場合、図１１Ｃは８連結の場合、図１１Ｄはラスタスキャンとルックアップテーブル５０、図１１Ｅは白い画素に対するラベル番号の振り方、図１１Ｆは最初に番号を割り振った様子、図１１Ｇは更にラスタスキャンを続ける様子、図１１Ｈは新たな番号（２）を割り振った様子、図１１Ｉは更に新たな番号（３）を割り振った様子、図１１Ｊはルックアップテーブル５０の書き換えの様子、図１１Ｋはラベリング処理において更にルックアップテーブル５０の書き換えをした様子、図１１Ｌは全ての画素に対してラベル番号が割り振られた状態、及び、図１１Ｍはラベル番号が修正された状態、をそれぞれ示す図である。

【0082】

前記図７Ａ及び図７Ｂに示すＲＯＧＭ４０のアルゴリズム１，２では、投票数が閾値よりも大きな部分に物体が存在すると推定して、そのセルに対して状態１か０を与えて２値画像を生成し、更に、その２値画像に対して収縮処理及び膨張処理を行っている。その後、図２ＡのステップＳ１０では、収縮・膨張処理の結果に対して、ラベリング処理を行うことによって、大きな物体の場合には複数のセルの集合から成るものとして登録し、小さな物体の場合には、最小で１個のセルから成る物体として登録することにより、従来の課題２の２を解決している。

【0083】

以下、図１１Ａ〜図１１Ｍを参照しつつ、ラベリング処理について説明する。２値化画像処理された画像において、白の部分（又は黒の部分）が連続した画素に同じ番号を割り振る処理をラベリング処理という。このラベリング処理は、通常、同じ番号毎の面積（画素数）や幅、高さ等の特徴量を求めて欠陥検査や分類処理等に用いられる。

【0084】

ラベリングには、図１１Ａのような２値化された画像の縦、横方向に連続している部分を同じラベルにする図１１Ｂのような４連結の場合（即ち、４近傍の場合）と、縦、横、斜め方向に連続している部分を同じラベルにする図１１Ｃのような８連結の場合（即ち、８近傍の場合）と、の２種類の処理がある。

【0085】

以下、８連結の場合において、ラベリング処理のアルゴリズムを説明する。
先ず、図１１Ｄに示すように、画像全ての画素のラベル番号を０（ゼロ）で初期化しておき、ラベリングで番号を割り付けるためのラベリング番号のルックアップテーブル５０を用意する。このルックアップテーブル５０において、Srcは、後述のラスタスキャン時に最初に割り振る番号を示し、Dstは、その後の番号の割り振りの際に、新たに余分につけてしまった番号を、より小さな番号に振り直すための番号を書き込む欄である。そして、画像の左上から右方向へラスタスキャンを行い、画素３６の色が白の位置を検索する。

【0086】

図１１Ｅに示すように、白の画素３６の左上、上、右上、左の画素３７のラベル番号を参照し、全て０（ゼロ）の場合は、最後に割り振った番号＋１のラベル番号を割り振る。もし、参照した画素３７のラベル番号が複数存在した場合は、最小の番号を割り振る。

【0087】

図１１Ｄのようにラスタスキャンを行って、最初に番号を割り振った様子が、図１１Ｆに示されている。同様に続行すると、図１１Ｇのようになる。さらにラスタスキャンを続けて、新たなラベル番号（２）を付けた様子が、図１１Ｈに示されている。そして、更にラスタスキャンを続けて新たなラベル番号（３）を振った様子が、図１１Ｉに示されている。

【0088】

図１１Ｊに示すように、参照した画素のラベル番号が複数存在した場合、最小の番号を割り振る。この時、使用しなかったラベル番号（図１１Ｊの例では３）に対応するルックアップテーブル５０のSrc（３）の右側のDstの欄の数字を、このとき割り振られた最小の番号（１）に書き換える。以後、同様にラスタスキャンを続けて、図１１Ｋに示すように、１を割り振る際に、右上の番号が「２」となっているため、最小の番号ではない値＝２に対応するSrcの欄の右隣のDstに、実際に割り振った値＝１を書き入れる。このようにして、図１３Ｌに示すように、全ての画素に対してラベル番号が割り振られる。

【0089】

次に、処理の途中でルックアップテーブル５０の番号を変更した番号を、得られた画像に対する番号に対して、例えば、「２→１、３→１、６→５」のように変更すると、図１１Ｍに示すように、連続した領域は同じ番号になり、ラベリングが完了する。

【0090】

以上のようなラベリングを、ＲＯＧＭ４０に適用することによって、大きな物体であっても小さな物体であっても、１つのラベル番号が与えられ、１つの物体として扱うことが可能になる。これにより従来の課題２を解決できる。
なお、以上の説明では、２値のマップに対してノイズを削除するために収縮処理及び膨張処理を行った後にラベリング処理をした。ここで、ノイズがないか又は非常に少ない場合には、収縮処理及び膨張処理を行わずに、２値のマップに対して直接、ラベリング処理を行ってもよい。
以下に、本発明の物体検出装置２０の実施例を示す。

【実施例】

【0091】

実施例の物体検出装置２０は、ステレオカメラ２１とこのステレオカメラ２１の出力側に接続されるコンピュータ（ＰＣと呼ぶ）から構成した。ＰＣ、つまり情報処理装置は、図１に示す視差算出部２２、距離・３Ｄ情報算出部２３、前方路面からのカメラ高さ・傾き推定部２４、前方路面に合わせた傾き変換部２５、ＯＧＭ算出部２６、ノイズ除去部２７、物体検出部２８、物体情報の出力部２９及びグローバルマップ表示部３０を構成する。ステレオカメラ２１とコンピュータは、インターフェースとなるＵＳＢ３．０を介して接続した。

【0092】

ステレオカメラ２１は、以下の構成を有している。
ステレオカメラ２１：ＺＭＰ社製、RoboVision（登録商標）2 カメラモジュール
ＣＭＯＳイメージセンサ：ソニー製、IMX224を２個使用
解像度：１２８０×９６０ピクセル（３０ｆｐｓ（frame per second））

【0093】

ＯＳとしてWindows 8.1（６４ビット）を搭載したＰＣを用いた。このＰＣに第１占有グリッドマップ及び冗長な第２占有グリッドマップを生成するソフトウェアをインストールした。ＰＣの主要な構成を以下に示す。
ＣＰＵ:インテル製 Core ^TMi7-5960X、８コア／3GHz
ＲＡＭ:１６ＧＢ
ＨＤＤ：２ＴＢ

【0094】

（比較例）
実施例と比較するために、第１占有グリッド（ＯＧＭ）マップを得る従来のソフトウェアをインストールした以外は、実施例１と同じステレオカメラ２１とＰＣを用いた。

【0095】

（課題１に対する本発明の効果）
課題１に対する本発明の効果を評価するために、ターゲットを床面に固定した状態で、少しずつステレオカメラ２１の位置を横にずらして行って、それぞれの位置でステレオ処理とＯＧＭ処理及びＲＯＧＭ処理を行った。ターゲットは、図示するようにロボットの絵が描かれた直方体の箱とした。
図１２（ａ）〜（ｃ）は、ステレオカメラ２１の位置を横方向で変えたときの実施例の画像を示し、上段が画像であり、下段が距離情報を階調処理した距離画像である。ここで、上段ではステレオカメラ２１の右カメラ２１Ｒの画像のみを示し、左カメラ２１Ｌの画像は省いている。図１２では、視差範囲を制限したために、正しい視差が求まっていない部分が手前側と奥側に存在している。グリッドサイズは、横方向及び奥行き方向で共に０．５ｍである。下段の距離画像には、手前と奥を示している。

【0096】

図１３は、実施例のＲＯＧＭ４０による箱の検出の投票数を示す図である。図１３では、ステレオカメラ２１の横位置の変化（６通り）を、箱が常に同じ位置、即ち横方向位置が０の位置となるように補正した場合のＲＯＧＭ４０の投票値を示す。これから横軸は補正後の横方向位置（ｍ）であり、縦軸は投票数である。図１３に示すように、箱がグリッドの境界線の近くでもセルの中央付近であっても、投票値はそれほど変わりがないことがわかる。これにより、箱の位置が、ＲＯＧＭ４０のどの位置にあっても、閾値を高くしていった際に安定して箱を検出できることがわかる。

【0097】

図１４は、比較例のＯＧＭ１０による箱の検出を示し、（ａ）は横方向の投票数を、（ｂ）は横方向及び奥行き（距離）方向の投票数を示す図である。ステレオカメラ２１の横位置の変化は、図１３と同じ６通りである。
図１４（ａ）に示すように、少しずつ横位置をずらした場合に、ＯＧＭ１０のグリッドの境界上に箱がある場合、又はグリッドの間（セルのど真ん中）にある場合によって、投票数が変化することが分かる。
図１４（ｂ）に示すように、奥行き（距離）方向の投票数からは、箱に対応するセルの位置が分かるが、箱の位置がグリッド境界に近いほど、投票数が少ないことがわかる。これにより、比較例のＯＧＭ１０では、実施例のＲＯＧＭ４０とは異なり閾値を上げていくと、境界付近で物体を検出できなくなってしまうことが分かる。

【0098】

（課題２の１に対する本発明の効果）
本発明の課題２の１に対する効果を評価するために、ノイズが生じている距離画像において、収縮と膨張処理によるノイズ除去について説明する。
図１５はノイズを調べるのに用いた画像を示し、（ａ）が右カメラ２１Ｒの画像を、（ｂ）が距離画像を示す図である。図１５（ｂ）は、図１２の下段と同様に階調処理した距離画像である。
図１５（ａ）に示すように、横断歩道に一人の歩行者と自転車に乗った人がおり、図１５（ｂ）に示すように、距離画像の上側の空の部分にステレオの誤対応によるノイズが生じている。

【0099】

図１６は、距離画像から得られる３Ｄ点群に対して、ＲＯＧＭ処理をした投票結果を示す図である。図１６の座標では、横方向及び奥行き（距離）方向に対して高さ方向に投票数を示している。図１６に示すように、投票数の多い歩行者と自転車に乗った人と共に、ノイズが生じていることが分かる。

【0100】

図１７は、図１６のデータを閾値が１００００として２値化した結果を示す図である。図１７の座標は、高さ方向が２値（０、１）を示す以外は、図１６と同じである。図１７に示すように、二つの山が歩行者と自転車に乗った人に対応すると共に、手前の山がノイズに対応していることが分かる。

【0101】

図１８は、図１７のデータを収縮・膨張処理した結果を示す図である。図１８の座標は、図１７と同じである。収縮・膨張処理は、収縮と膨張をそれぞれ１回行った。図１８に示すように、収縮・膨張処理により図１７で生じたノイズによる山が除去されたことが分かる。

【0102】

（課題２の２に対する本発明の効果）
課題２の２に対する本発明の効果を評価するために、図１８に示すノイズを除去した２値データのラベリング処理を行った。図１８に示すように、この段階では、歩行者に対応する単に１の値のセルが並んでいる山が２つあり、この２つの山のそれぞれが、一つずつの塊として分けられていない。

【0103】

図１９は、図１８の２値データをラベリング処理した結果を示す図である。図１９の座標は、高さ方向をラベルとした以外は図１８と同じである。図１９に示すように、ラベリング処理により、自転車に乗った人の塊を構成するすべてのセルに対しては、高さ方向の値、つまり１のラベルが与えられ、中央の歩行者の塊を構成するすべてのセルに対しては、高さ方向の値が２というラベルが割り振られていることが分かる。これにより、課題２の２の複数のセルから成る一つの塊に対して、ラベリング処理によって１つのラベリング値が与えられ、１つの物体として検出し取り扱うことが可能となる。

【0104】

（課題３に対する本発明の効果）
課題３に対する本発明の効果を評価するために、左右画像に対して３Ｄ値を求め、３Ｄ値から投票時に距離の自乗を加算した実施例のＲＯＧＭ４０によるマップと従来のＯＧＭ１０によるマップを求めた。
図２０は、遠方の投票を調べるのに用いた画像を示し、（ａ）が左画像を、（ｂ）が右画像を、（ｃ）は距離画像である。図２０（ｃ）は、図１２の下段と同様に階調処理した距離画像である。
図２０に示すように、前方には図１２と同じロボットの絵が描かれた直方体の箱があり、この箱の位置を、前方２ｍから４．５ｍ迄０．５ｍ毎に変えたときのＲＯＧＭ４０及びＯＧＭ１０によるマップを求めた。

【0105】

図２１は、実施例のＲＯＧＭ４０によるマップである。図２１では、横方向及び奥行き（ｍ）に対して高さ方向に投票数を示している。図２１に示すように、箱の位置を０．５ｍ毎に変化させたとき、ある程度のバラツキはあるものの遠方の箱の投票数も多くなり、距離による投票数の変化が後述する従来のＯＧＭ１０よりも明らかに改善されていることが判明した。

【0106】

図２２は、比較例のＯＧＭ１０によるマップである。図２２の座標は図２１と同じである。図２２に示すように、比較例のＯＧＭ１０では距離に関係なく投票しているため、距離が遠くなると投票数も減っていることが分かる。

【0107】

（課題４に対する本発明の効果）
課題４に対する本発明の効果を評価するために、遠方に停車した車両に対して３Ｄ値を求め、３Ｄ値から実施例のＲＯＧＭ４０によるマップと従来のＯＧＭ１０によるマップを求めた。
図２３は、遠方の車両を調べるのに用いた画像を示し、（ａ）が左画像を、（ｂ）は距離画像を示す図である。図２３（ｂ）は、図１２の下段と同様に階調処理した距離画像である。図２３に示すように、前方に停止した車両と道路の両側には樹木があることが分かる。

【0108】

図２４は、実施例の３Ｄ値から求めたＲＯＧＭ４０の結果を示す図である。図２４の座標は、図２２と同じである。図２２のＲＯＧＭ４０は、投票時に距離の自乗を加算すると共に、視差誤差を考慮した式（２）に基づいて、奥行方向に投票するセルの範囲を広げる処理を行った。図２４に示すように、遠方の物体に投票数が多い物体が観測される。なお、手前にある山は、明るい空のテクスチャが殆どなかったため、誤対応によるノイズを拾ったものである。

【0109】

図２５は、図２４のデータを２値化した結果を示す図である。図２５の座標は、高さ方向が２値（０、１）を示す以外は、図２４と同じである。図２５に示すように、遠方に停止した車両を含む物体と、手前にノイズが生じていることが分かる。

【0110】

図２６は、図２５のデータを収縮・膨張処理した結果を示す図である。図２６の座標は、図２５と同じである。収縮・膨張処理は、収縮と膨張をそれぞれ１回行った。図２６に示すように、収縮・膨張処理により図２５で生じたノイズによる山が除去されたことが分かる。これにより、本発明の物体検出方法によれば、従来検出できなかった遠方の物体を検出することが可能となった。

【0111】

実施例のように遠方に停止した車両を、ＯＧＭ１０を使って検出しようとしたが、ラベリング後に検出できなかった。

【0112】

遠方に停止した車両を、実施例とは異なり、距離の自乗の加算と式（２）に基づいた処理をしないＲＯＧＭ４０だけを使って検出しようとしたが、２値化後に収縮・膨張処理を行ったが何も検出されなかった。

【0113】

以上説明したように、上記実施例及び比較例によれば、課題１〜４を解決できることが判明した。

【0114】

本発明は、上記実施の形態に限定されるものではなく、特許請求の範囲に記載した発明の範囲内で種々の変形が可能であり、それらも本発明の範囲内に含まれることはいうまでもない。

【符号の説明】

【0115】

１０ ＯＧＭ
２０ 物体検出装置
２１ ステレオカメラ
２２ 視差算出部
２３ 距離・３Ｄ情報算出部
２４ 前方路面からのカメラ高さ・傾き推定部
２５ 前方路面に合わせた傾き変換部
２６ ＯＧＭ算出部
２７ ノイズ除去部
２８ 物体検出部
２９ 検出物体情報の出力部
３０ グローバルマップ表示部
４０ ＲＯＧＭ
５０ ルックアップテーブル

【図1】

【図2A】

【図2B】

【図3A】

【図3B】

【図4A】

【図4B】

【図5】

【図6A】

【図6B】

【図6C】

【図6D】

【図7A】

【図7B】

【図8】

【図9】

【図10】

【図11A】

【図11B】

【図11C】

【図11D】

【図11E】

【図11F】

【図11G】

【図11H】

【図11I】

【図11J】

【図11K】

【図11L】

【図11M】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】

【図19】

【図20】

【図21】

【図22】

【図23】

【図24】

【図25】

【図26】

【図27】

【図28】