(57)【要約】      （修正有）【課題】オフロード環境で使用される車両においても、走行可能領域を容易に判断できるようにする。【解決手段】走行可能領域検出装置１００は、車両に搭載され左右に離して配置した２台の撮像部１０１，１０２を有するステレオカメラと、撮像部が撮像した左右の画像を処理する計測演算装置１４０とを備える。計測演算装置は、地表面を予め定めた地球座標系で表現される複数の三角パッチからなる三角メッシュで構成し、一方の撮像部が撮影した地表面の基準画像と他方の撮像部が撮像した地表面の他方画像とに各三角パッチを投影し、基準画像と他方画像中に投影した同一の前記三角パッチの画素値の差分が最小となるよう三角パッチの頂点の上下方向を移動パラメータとして地表面の三次元サーフェスを特定する。特定した三次元サーフェスから走行可能領域を検出する。【選択図】図２
2014228941.tif 000002
【特許請求の範囲】
【請求項１】
車両に搭載され左右に離して配置した２台の撮像部を有するステレオカメラと、前記撮像部が撮像した左右の画像を処理する計測演算装置とを備え、前記計測演算装置は、地表面を予め定めた地球座標系で表される頂点座標を持つ複数の三角パッチで構成される三角メッシュによって表現し、前記各三角パッチを前記左右の画像の一方の基準画像ともう一方の他方画像とに投影し、前記三角メッシュで表現された地表面の同一点を投影した前記基準画像上および前記他方画像上の画素の値の差分が最小となるように、前記三角パッチの頂点の上下方向を移動パラメータとして地表面の三次元サーフェスを特定することを特徴とする地表面３次元サーフェス形状計測装置。
【請求項２】
前記画素値の差分は、輝度差の二乗和（ＳＳＤ）であることを特徴とする請求項１に記載の地表面３次元サーフェス形状計測装置。
【請求項３】
前記計測演算装置は、画素値や勾配の小さい画素を利用しないことを特徴とする請求項１に記載の地表面３次元サーフェス形状計測装置。
【請求項４】
前記２台の撮像部が撮像した前記基準画像と前記他方画像間の明るさの違いを補償する手段を設けたことを特徴とする請求項１に記載の地表面３次元サーフェス形状計測装置。
【請求項５】
前記移動パラメータの特定における繰返し計算の計算式に，平滑化拘束項を付与したことを特徴とする請求項１に記載の地表面３次元サーフェス形状計測装置。
【請求項６】
前記繰返し計算の計算式に更新ノルム拘束項を付与したことを特徴とする請求項５に記載の地表面３次元サーフェス形状計測装置。
【請求項７】
前記繰返し計算においてはオクルージョンチェックを実行し、このオクルージョンチェックに基づいて画素毎の使用フラグを付与することを特徴とする請求項１に記載の地表面３次元サーフェス形状計測装置。
【請求項８】
前記繰返し計算においては、計測演算装置が階層的推定を行うことを特徴とする請求項１に記載の地表面３次元サーフェス形状計測装置。
【請求項９】
前記計測演算装置は路肩候補抽出部を有し、この路肩候補抽出部は各パッチの各頂点座標から求めた法線方向に基づいて走行可能領域を区切る路肩候補を判断することを特徴とする請求項１に記載の地表面３次元サーフェス形状計測装置。
【請求項１０】
前記計測演算装置は、前記２台の撮像部が撮像した２つの画像の視差情報を算出し、視差情報から前記２台の撮像部に撮像される物体までの距離情報を算出する距離計測部を有し、前記繰り返し計算の初期値に、前記距離計測部が計測した距離情報を使用することを特徴とする請求項９に記載の地表面３次元サーフェス形状計測装置。
【請求項１１】
車両に搭載され左右に離して配置した２台の撮像部を有するステレオカメラと、前記撮像部が撮像した左右の画像を処理する計測演算装置とを備え、前記計測演算装置は、路肩候補抽出部と路肩候補連続性判定部とを備え、地表面を予め定めた地球座標系で表される頂点座標を持つ複数の三角パッチで構成される三角メッシュによって表現し、前記各三角パッチを前記左右の画像の一方の基準画像ともう一方の他方画像とに投影し、前記三角メッシュで表現された地表面の同一点を投影した前記基準画像上および前記他方画像上の画素の値の差分が最小となるように、前記三角パッチの頂点の上下方向を移動パラメータとして地表面の三次元サーフェスを特定し、前記路肩候補抽出部は各パッチの各頂点座標から求めた法線方向に基づいて走行可能領域を区切る路肩候補を判断し、前記路肩候補連続性判定部が判断した路肩候補から、各パッチの法線方向が予め定めた地球座標系のｚ＝０の平面となす角が所定値以上であり、その所定値以上となるパッチの数が予め定めた数以上連続する場合に、その所定値以上となる連続するパッチを路肩と判定することを特徴とする走行可能領域検出装置。
【請求項１２】
車両に搭載され左右に離して配置した２台の撮像部を有するステレオカメラと、前記撮像部が撮像した左右の画像を処理する計測演算装置とを備え、前記計測演算装置は、路肩候補抽出部と外部情報利用判定部と地図データの記憶手段とＧＰＳを用いて自車位置を計測する自車位置計測装置と路肩出力部とを備え、地表面を予め定めた地球座標系で表される頂点座標を持つ複数の三角パッチで構成される三角メッシュによって表現し、前記各三角パッチを前記左右の画像の一方の基準画像ともう一方の他方画像とに投影し、前記三角メッシュで表現された地表面の同一点を投影した前記基準画像上および前記他方画像上の画素の値の差分が最小となるように、前記三角パッチの頂点の上下方向を移動パラメータとして地表面の三次元サーフェスを特定し、前記路肩候補抽出部は各パッチの各頂点座標から求めた法線方向に基づいて走行可能領域を区切る路肩候補を判断し、前記外部情報利用判定部は地図データ記憶手段に記憶された地図データと前記自車位置計測装置に記憶された自車位置データを参照して、自車前方に路肩が不連続になる部分の有無を判断し、前記路肩出力部は路肩および路肩の不連続部を出力することを特徴とする走行可能領域検出装置。
【請求項１３】
車両に搭載され左右に離して配置した２台の撮像部を有するステレオカメラと、前記撮像部が撮像した左右の画像を処理する計測演算装置とを備え、前記計測演算装置は、地表面を予め定めた地球座標系で表される頂点座標を持つ複数の三角パッチで構成される三角メッシュによって表現し、前記各三角パッチを前記左右の画像の一方の基準画像ともう一方の他方画像とに投影し、前記三角メッシュで表現された地表面の同一点を投影した前記基準画像上および前記他方画像上の画素の値の差分が最小となるように、前記三角パッチの頂点の上下方向を移動パラメータとして地表面の三次元サーフェスを特定し、特定した三次元サーフェスの各三角パッチの法線が地球座標系のｚ＝０の平面となす角から走行可能領域を検出する計測演算装置とを備えた走行可能領域検出装置を、少なくとも１個前面側に搭載したことを特徴とする不整地を走行可能な建設機械。
【請求項１４】
車両に搭載され左右に離して配置した２台の撮像部を有するステレオカメラと、前記撮像部が撮像した左右の画像を処理する計測演算装置とを用いて走行可能領域を検出する方法であって、地表面を予め定めた地球座標系で表される頂点座標を持つ複数の三角パッチで構成される三角メッシュによって表現し、前記各三角パッチを前記左右の画像の一方の基準画像ともう一方の他方画像とに投影し、前記三角メッシュで表現された地表面の同一点を投影した前記基準画像上および前記他方画像上の画素の値の差分が最小となるように、前記三角パッチの頂点の上下方向を移動パラメータとして地表面の三次元サーフェスを特定し、特定した三次元サーフェスの各三角パッチが地球座標系でｚ＝０の面となす角が所定値以上であれば路肩候補とし、路肩候補の三角パッチが所定数以上連続すれば路肩と判定することを特徴とする走行可能領域検出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、地表面の３次元サーフェス形状を計測する装置または車両が走行可能な走行路を検出する走行可能領域検出装置およびそれを搭載した建設機械並びに走行可能領域検出方法に係り、特に不整地を走行可能な建設機械に用いて好適な走行可能領域検出装置およびそれを搭載した建設機械並びに走行可能領域検出方法に関する。
【背景技術】
【０００２】
従来から、車両周囲で起こり得る危険な事象を早期に検出し、検出した危険な事象を回避して車両の安全な走行を実現するために、車両の操舵やアクセル、ブレーキを自動制御する装置が種々提案されている。そして、一部の車両ではこの種の危険回避装置が既に搭載されている。
【０００３】
また、不整地等を走行する鉱山用のダンプトラック等においては、稼働率向上のために自動走行も実現しており、その場合自動走行を可能にするために、車両の前方の走行可能な領域や障害物を検出するセンサーを車両に取り付けている。センサーとしては、カメラやレーダと、カメラやレーダが検出した信号を処理する信号処理装置が使用される。
【０００４】
さらに、鉱山用ダンプトラック等では、走行場所がオフロード環境であり、凹凸や勾配が多いので、走行可能な領域と障害物とを分離して検出する必要がある。このようなオフロード環境下を走行する車両が、走行路面を正確に検出することが特許文献１や特許文献２に開示されている。また、ブラインドカーブ等の見通しの悪い環境で運転性を向上させることが、特許文献３に記載されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００８−１２３０１９号公報
【特許文献２】特開２００９−１３９３２３号公報
【特許文献３】特開平１１−１４８３９４号公報
【非特許文献】
【０００６】
【非特許文献１】Sugimoto, S. and Okutomi, M., ”A direct and Efficient Method for Piecewise-Planar Surface Reconstruction from Stereo Images, IEEE Computer Society Conference on Computer Vision and Pattern Recognition, pp. 1-8, 2007
【発明の概要】
【発明が解決しようとする課題】
【０００７】
特許文献１には、基準画像と参照画像の複数の画像を用いるときに、基準画像にメッシュを作成し３次元サーフェスを高速で作成することが開示されている。その際、繰返し計算によりメッシュの奥行きパラメータを計算で求めている。しかしながらこの方法によれば繰返し計算により、過度なサーフェス変化が生じ、計算コストの上昇を引き起こす恐れがある。
【０００８】
また、特許文献２に記載の車両用走行路面検出装置では、ステレオカメラが撮像した路面画像から互いの対応点を検出して、特定平面に隣接する平面の平面定数を勾配変化から推定している。しかしながら、この公報に記載のものは道路があることが前提であり、オフロード環境等の不整地で撮像した画面から道路等の走行可能領域を検出することについては、考慮されていない。
【０００９】
さらに特許文献３に記載の車両制御装置では、見通し距離を測定する際に、同様に急カーブ等が出現しても条件により見通し距離の差があることから、正確にブラインドカーブを認識できることが記載されている。しかしながらこの公報においても、オフロード環境で撮像した画面から走行可能領域を検出することについては、考慮されていない。
【００１０】
本発明は上記従来技術の不具合に鑑みなされたものであり、その目的は、オフロード環境で使用される車両においても、走行可能領域を容易に判断できるようにすることである。さらに、路面の凹凸や勾配があるオフロード環境を走行する建設機械等の車両を効果的に運転して、車両運転者の負担を軽減することも目的とする。
【課題を解決するための手段】
【００１１】
上記目的を達成するために本発明では、地表面３次元サーフェス形状計測装置が、車両に搭載され左右に離して配置した２台の撮像部を有するステレオカメラと、前記撮像部が撮像した左右の画像を処理する計測演算装置とを備え、前記計測演算装置は、地表面を予め定めた地球座標系で表される頂点座標を持つ複数の三角パッチで構成される三角メッシュによって表現し、前記各三角パッチを前記左右の画像の一方の基準画像ともう一方の他方画像とに投影し、前記三角メッシュで表現された地表面の同一点を投影した前記基準画像上および前記他方画像上の画素の値の差分が最小となるように、前記三角パッチの頂点の上下方向を移動パラメータとして地表面の三次元サーフェスを特定するようにしたことを特徴とする。
【００１２】
また走行可能領域検出装置が、車両に搭載され左右に離して配置した２台の撮像部を有するステレオカメラと、前記撮像部が撮像した左右の画像を処理する計測演算装置とを備え、前記計測演算装置は、路肩候補抽出部と路肩候補連続性判定部とを備え、地表面を予め定めた地球座標系で表される頂点座標を持つ複数の三角パッチで構成される三角メッシュによって表現し、前記各三角パッチを前記左右の画像の一方の基準画像ともう一方の他方画像とに投影し、前記三角メッシュで表現された地表面の同一点を投影した前記基準画像上および前記他方画像上の画素の値の差分が最小となるように、前記三角パッチの頂点の上下方向を移動パラメータとして地表面の三次元サーフェスを特定し、前記路肩候補抽出部は各パッチの各頂点座標から求めた法線方向に基づいて走行可能領域を区切る路肩候補を判断し、前記路肩候補連続性判定部が判断した路肩候補から、各パッチの法線方向が予め定めた地球座標系のｚ＝０の平面となす角が所定値以上であり、その所定値以上となるパッチの数が予め定めた数以上連続する場合に、その所定値以上となる連続するパッチを路肩と判定するようにしたことを特徴とする。
【００１３】
さらに走行可能領域検出装置が、車両に搭載され左右に離して配置した２台の撮像部を有するステレオカメラと、前記撮像部が撮像した左右の画像を処理する計測演算装置とを備え、前記計測演算装置は、路肩候補抽出部と外部情報利用判定部と地図データの記憶手段とＧＰＳを用いて自車位置を計測する自車位置計測装置と路肩出力部とを備え、地表面を予め定めた地球座標系で表される頂点座標を持つ複数の三角パッチで構成される三角メッシュによって表現し、前記各三角パッチを前記左右の画像の一方の基準画像ともう一方の他方画像とに投影し、前記三角メッシュで表現された地表面の同一点を投影した前記基準画像上および前記他方画像上の画素の値の差分が最小となるように、前記三角パッチの頂点の上下方向を移動パラメータとして地表面の三次元サーフェスを特定し、前記路肩候補抽出部は各パッチの各頂点座標から求めた法線方向に基づいて走行可能領域を区切る路肩候補を判断し、前記外部情報利用判定部は地図データ記憶手段に記憶された地図データと前記自車位置計測装置に記憶された自車位置データを参照して、自車前方に路肩が不連続になる部分の有無を判断し、前記路肩出力部は路肩および路肩の不連続部を出力するようにしたことを特徴とする。
【００１４】
また不整地を走行可能な建設機械が、左右に離して配置した２台の撮像部を有するステレオカメラと、地表面を予め定めた地球座標系で表現される複数の三角パッチからなる三角メッシュで構成し、一方の前記撮像部が撮影した地表面の基準画像と他方の前記撮像部が撮像した地表面の他方画像とに前記各三角パッチを投影し、前記基準画像と前記他方画像中に投影した同一の前記三角パッチの位置を示す画素値の差分が最小となるよう前記三角パッチの頂点の上下方向を移動パラメータとして地表面の三次元サーフェスを特定し、特定した三次元サーフェスの各三角パッチの法線が地球座標系のｚ＝0の平面となす角から走行可能領域を検出する計測演算装置とを備えた走行可能領域検出装置を、少なくとも１個前面側に搭載したことを特徴とする。
【００１５】
さらに走行可能領域検出方法は、車両に搭載され左右に離して配置した２台の撮像部を有するステレオカメラと、前記撮像部が撮像した左右の画像を処理する計測演算装置と用いて走行可能領域を検出する方法であって、地表面を予め定めた地球座標系で表現される複数の三角パッチからなる三角メッシュで構成し、一方の前記撮像部が撮影した地表面の基準画像と他方の前記撮像部が撮像した地表面の他方画像とに前記各三角パッチを投影し、前記基準画像と前記他方画像中に投影した同一の前記三角パッチの位置を示す画素値の差分が最小となるよう前記三角パッチの頂点の上下方向を移動パラメータとして地表面の三次元サーフェスを特定し、特定した三次元サーフェスの各三角パッチが地球座標系でｚ＝０の面となす角が所定値以上であれば路肩候補とし、路肩候補の三角パッチが所定数以上連続すれば路肩と判定する。
【発明の効果】
【００１６】
本発明によれば、車両前方の３次元サーフェスをステレオカメラで撮像し、その撮像画面に基づいて３次元サーフェスを二次元メッシュに分割し、二次元メッシュの各メッシュの法線方向を検出することにより、オフロード環境等下であっても走行可能な領域の左右両境界をなす路肩を容易に検出することが可能になる。これにより、路面の凹凸や勾配があるオフロード環境においても、車両を効果的に運転できるとともに、車両の運転者の負担を軽減できるとともに、運転手を必要としない無人運転も可能となる。
【図面の簡単な説明】
【００１７】
【図１】本発明に係る走行可能領域検出装置を搭載した建設機械の一実施例の斜視図及び走行可能領域検出装置の検出画面の処理を説明する図である。
【図２】図１に示した建設機械が有する走行可能領域検出装置のブロック図である。
【図３】３次元サーフェス生成を説明する図である。
【図４】各パラメータ間の関係を説明する図である。
【図５】図２に示した走行可能領域検出装置が有する路肩候補抽出部の処理を示すフローチャートである。
【図６】図２に示した走行可能領域検出装置が有する路肩候補連続性判定部の処理を示すフローチャートである。
【図７】図２に示した走行可能領域検出装置が有する外部情報利用判定部の処理を示すフローチャートである。
【図８】図２に示した走行可能領域検出装置が有するステレオカメラによる測定を模式的に示した図である。
【図９】ステレオカメラでの物体探索を説明する図である。
【図１０】ステレオカメラにおける物体特定を説明する図である。
【発明を実施するための形態】
【００１８】
以下、本発明に係る走行可能領域の検出装置および検出方法のいくつかの実施例を、図面を用いて説明する。なお、以下の説明では、ダンプトラック８０が不整地を走行する際に、このダンプトラック８０に搭載したステレオカメラ１３０（１０１、１０２）の映像を用いて、車両８０の前方の３次元サーフェス７０を計測し、路肩６１、６２を検出する場合を例にとる。また本発明に係る走行可能領域検出装置１００は、ダンプトラック等が不整地を走行する場合以外にも、一般の車両が公道を走行する際にも適用できることは言うまでもない。
【００１９】
図１は、建設機械の一種であるダンプトラック８０が路肩を検出する様子を模擬した図であり、図１(ａ)は、３次元サーフェス７０を構成する路肩６１、６２で挟まれた路面(走行可能領域)６０を、ダンプトラック８０が走行する様子を示した斜視図である。ダンプトラック８０には、左撮像部１０１及び右撮像部１０２を有するステレオカメラ１３０(図１では図示せず)が搭載されている。なお、本図では左撮像部１０１及び右撮像部１０２を別体として表現しているが、左撮像部１０１と右撮像部１０２とが、一体ケーシング内に収容されて、ステレオカメラ１３０を構成してもよい。ダンプトラック８０は、上下方向に起伏し、路肩６１，６２で挟まれた走行可能領域６０だけを走行する。
【００２０】
図１(ｂ)に、ステレオカメラ１３０が撮像した３次元サーフェス７０から走行可能領域６０を自動検出するために、３次元サーフェスをメッシュでモデル化する様子を、模式的に示す。図の上側は走行可能領域６０の奥行き側で、図の下側はダンプトラック８０に近い走行可能領域６０を示している。左右のハッチング部分６１，６２は、盛土で形成された路肩を示している。詳細を後述するように、この路肩６１，６２の部分及び走行可能領域６０の撮像画像から、メッシュを作成して走行可能領域６０を自動で検出する。
【００２１】
以下に、走行可能領域６０の具体的な自動検出方法について説明する。図２は、本発明に係る走行可能領域検出装置１００の一実施例のブロック図である。本実施例の走行可能領域検出装置１００は、走行可能領域６０の両側に盛土で形成される路肩６１、６２を検出するもので、路肩検出装置である。
【００２２】
路肩検出装置１００は、大別して撮像手段(ステレオカメラ)１３０と計測演算装置１４０とから構成される。撮像手段１３０は、左撮像部１０１と右撮像部１０２とを有しており、この撮像手段１３０が搭載された車両の前方視野を撮像する。なお、本実施例ではステレオカメラ１３０と計測演算装置１４０を別体としているが、ステレオカメラ１３０と計測演算装置１４０とを一体化して、ステレオカメラとしてもよい。
【００２３】
計測演算装置１４０は、距離算出部１０３と、３Ｄサーフェス生成部１０４と、路肩候補抽出部１０５と、路肩候補連続性判定部１０６と、外部情報利用判定部１０７と、路肩出力部１０９とを有する制御装置１２０を備えている。
【００２４】
距離算出部１０３には、左撮像部１０１で撮像した画像５０２と、右撮像部１０２で撮像した画像５０３が入力される（図８参照）。そして、同一対象物を左撮像部１０１が撮像した画像５０２と右撮像部１０２が撮像した画像５０３上のズレから左右画像５０２，５０３における対象物の画素位置を求めて、撮像した対象物までの距離を算出する。算出した距離情報は、以下に説明する３Ｄサーフェス生成部１０４に入力される。
【００２５】
３Ｄサーフェス生成部１０４には、撮像手段１３０の左右撮像部１０１、１０２が撮像した画像５０２、５０３が入力される。３Ｄサーフェス生成部１０４は、入力された撮像画面に示される車両８０の前方の地面７０の３次元形状から、複数の三角形の二次元メッシュ８２を生成し、地面７０をメッシュモデル化する（図３参照）。そして、モデル化された二次元メッシュ８２の各々が有する頂点８１の３次元座標と各二次元メッシュ８２の法線ベクトルを計算する。その際、各二次元メッシュ８２の各頂点８１の収束計算をするのに、初期値として距離算出部１０３が算出した距離情報を利用する。
【００２６】
路肩候補抽出部１０５は、撮像手段１３０が撮像した地面(３次元サーフェス)７０の画像から３Ｄサーフェス生成部１０４が生成した各二次元メッシュ８２について、法線ベクトルを求める。法線ベクトルの値が予め定めた値以上になったときに、路肩候補抽出部１０５は、その二次元メッシュ８２を路肩の候補として抽出する。ここで、大型ダンプトラックは鉱山内を走行しているものとし、路肩はこの鉱山内の搬送路の両側に形成される路肩である。路肩は、走行可能な領域６０の境界線であって、所定の高さで盛土されており、直線または曲線状に形成されている。
【００２７】
路肩候補連続性判定部１０６は、路肩候補抽出部１０５が抽出した路肩の候補となる二次元メッシュ８２から、抽出されたメッシュ８２が所定個数以上連続する場合に、その連続するグループを路肩６１，６２として判定する。この条件を満たさない場合には、路肩候補抽出部１０５が抽出した路肩候補の二次元メッシュ８２をノイズと判断する。
【００２８】
外部情報利用判定部１０７は、制御装置１４０が有する記憶手段に記憶された車両８０が走行する地図１０８の情報と、この車両８０に備えられた自車位置計測装置１１２が計測した自車位置の情報から、自車８０の前方に交差点が存在するか否かを判定する。次に、路肩候補連続性判定部１０６が路肩候補と判定した二次元メッシュ８２の路肩６１，６２としての信頼性を判定する。路肩６１，６２として信頼性が高いと判定した二次元メッシュ８２についての情報を、路肩出力部１０９に出力する。
【００２９】
路肩出力部１０９は、外部情報利用判定部１０７が路肩６１，６２として信頼性が高いと判定した二次元メッシュ８２の情報から路肩６１，６２を検出するとともに、計測された自車位置と検出した路肩６１，６２の相対位置関係を車両制御量作成部１１０へ出力する。
【００３０】
車両制御量作成部１１０では、路肩検出部１０９の出力である自車８０と路肩６１，６２の相対位置関係を用いて、自車８０が走行可能な領域６０から逸脱しないように、車両８０を自動制御するブレーキ制御量とステアリング制御量を計算する。そして、計算したブレーキ制御量信号とステアリング制御量信号を、図示しないブレーキ制御装置およびステアリング制御装置に送信する。
【００３１】
なお、上記構成において、計測演算装置１４０が有する制御装置１２０は、マイコン及びそれに搭載したプログラムであり、地図データ１０８は制御装置に付設した記憶手段に記憶されている。また、自車位置計測装置１１２は、詳細を後述するＧＰＳ等を利用した計測手段である。さらに、車両制御量作成部１１０は、アナログ信号またはデジタル信号を発生する信号発生手段を有している。
【００３２】
次に、ダンプトラック８０に搭載したステレオカメラ１３０が撮像したステレオ画像から地表面の３次元サーフェス７０を作成する手法について、図３及び図４を用いて説明する。図３は、地表面８５に作成するメッシュ８３及び地上座標系９０とステレオカメラ１０１、１０２を用いた時のカメラ座標系９１、９２の関係を示す模式図である。図４は、メッシュパラメータ及び平面パラメータ、平面射影変換パラメータの関係を示す模式図である。
（地表面のダイレクトサーフェス生成）
【００３３】
【数１】
2014228941.tif 000003
【００３４】
図３に示すように、地上座標系と基準カメラ座標系の関係を、
【００３５】
【数２】
2014228941.tif 000004
【００３６】
基準カメラ座標系と参照カメラ座標系の関係を
【００３７】
【数３】
2014228941.tif 000005
【００３８】
地上座標系のｘ-ｙ平面上に、三角パッチで構成されたメッシュ８２を定める。メッシュの例として、図３では、正方格子を用いたメッシュを示しており、この場合、メッシュの頂点８１は、Digital Elevation Map(DEM)と同様に、地平面上に規則的に配置されたものとなる。以下では、メッシュを構成する三角パッチをτｐ,（ｐ=１，２,…,Ｐ）で表わし、メッシュ頂点８１の地上座標系における３次元座標を
【００３９】
【数４】
2014228941.tif 000006
【００４０】
このとき、各頂点のｘｖ、ｙｖは定めたメッシュによって既知とする一方で、ｚｖの値はサーフェスの生成のための目的パラメータとし、
【００４１】
【数５】
2014228941.tif 000007
【００４２】
を目的パラメータベクトルとする。
（コスト関数）
本発明の手法では、繰り返し計算を利用して
【００４３】
【数６】
2014228941.tif 000008
【００４４】
を最適化する。ただし、
【００４５】
【数７】
2014228941.tif 000009
【００４６】
と更新されるものとする。
【００４７】
本手法では、
【００４８】
【数８】
2014228941.tif 000010
【００４９】
を算出するために、次式のコスト関数を利用する。
【００５０】
【数９】
2014228941.tif 000011
【００５１】
ただし、ＣＤ，ＣＳ，ＣＵはデータ項、平滑化項、更新ノルム拘束項をそれぞれ表わし、λＳ，λＵは、平滑化項と更新ノルム拘束項に対する重みをそれぞれ表わす。以下では、上記コスト関数を構成する３つの項について詳細を述べる。
(データ項ＣＤ)
基準画像をＴ、参照画像をＩとし、基準画像上と参照画像上の座標をそれぞれ
【００５２】
【数１０】
2014228941.tif 000012
【００５３】
ただし、以下では、煩雑さを避けるため、
【００５４】
【数１１】
2014228941.tif 000013
【００５５】
内部パラメータが正規化された座標(正規化画像座標)を示すものとする。
【００５６】
データ項ＣＤは、次式で示すように、２枚の画像の画素値のＳＳＤ（Sum of Squared Differences：輝度差の二乗和）とする。
【００５７】
【数１２】
2014228941.tif 000014
【００５８】
ｐ番パッチを基準画像に投影した際に、その投影三角形に含まれる基準画像の画素を表し、
【００５９】
【数１３】
2014228941.tif 000015
【００６０】
その画素を使用するか否かを示す１または０の値を表す（画素使用判定については、後述する）。また、
【００６１】
【数１４】
2014228941.tif 000016
【００６２】
ｐ番目の三角パッチを基準画像に投影した三角形内の画像座標と、同パッチを参照画像に投影した三角形内の画像座標との間に成立する平面射影変換を表す。より具体的には、
【００６３】
【数１５】
2014228941.tif 000017
【００６４】
次式（３）のように書ける。
【００６５】
【数１６】
2014228941.tif 000018
【００６６】
３×３の平面射影変換行列を示す。また、
【００６７】
【数１７】
2014228941.tif 000019
【００６８】
ｐ番パッチの基準カメラ座標系における３×１の平面パラメータベクトル、ｐ番パッチの地上座標系における３×１の平面パラメータベクトルをそれぞれ表わし、
【００６９】
【数１８】
2014228941.tif 000020
【００７０】
であることを示している
【００７１】
【数１９】
2014228941.tif 000021
【００７２】
ｐ番パッチに関わる３頂点の高さのみに依存する）。まとめると、図４に示すように、
【００７３】
【数２０】
2014228941.tif 000022
【００７４】
と書ける。ただし、
【００７５】
【数２１】
2014228941.tif 000023
【００７６】
ベクトル値関数であることを表している。
【００７７】
式（２）で示したデータ項は、ダイレクトイメージアライメントのための一般的なコストを、本発明の目的パラメータを用いて定式化したものである。しかし、この定式化をそのまま利用すると、最適化計算の計算コストが非常に大きくなる。そこで本発明の手法では、ＩＣＩＡ(Inverse Compositional Image Alignment)の考え方を利用して、式（２）を書き換える。書き換えたデータ項は次式（５）のように表わされる。
【００７８】
【数２２】
2014228941.tif 000024
【００７９】
式（５）の定式化には、
【００８０】
【数２３】
2014228941.tif 000025
【００８１】
の導出が必要である。ただし、後述するように、最適化では平面射影変換
【００８２】
【数２４】
2014228941.tif 000026
【００８３】
が一次近似されるため、その一次近似が得られれば十分である。最適化アルゴリズムについては、後述する。
（平滑化項ＣＳ）
上述のデータ項ＣＤのみを利用した場合、テクスチャのない三角パッチが多数存在すると、サーフェス生成は不良設定問題となる。また、図３で示したように、ステレオカメラが地面を斜め上から観測するように設置され、かつ地上座標系に定めたメッシュが規則性を持つ場合、カメラから遠方にあるパッチほど、それを画像上に投影した三角パッチは小さくなる。そのサイズがあまり小さいと、三角パッチの平面姿勢を決めるためのテクスチャ情報が不足し、テクスチャレスのケースと同様に、不良設定問題となる。平滑化項は、このような問題を回避するために定める。
【００８４】
本発明における平滑化項ＣＳは次式で表わされる。
【００８５】
【数２５】
2014228941.tif 000027
【００８６】
ｖ番の頂点に対して定められた重み係数ベクトルを示す。
【００８７】
【数２６】
2014228941.tif 000028
【００８８】
の代表例として、正方格子メッシュに対するラプラシアンオペレータが挙げられる。隣接点を８近傍とするラプラシアンオペレータの場合、
【００８９】
【数２７】
2014228941.tif 000029
【００９０】
は次式のように表わされる。
【００９１】
【数２８】
2014228941.tif 000030
【００９２】
ｖ番頂点の８近傍を表す。すなわち、このケースでは、
【００９３】
【数２９】
2014228941.tif 000031
【００９４】
ｖ番目の要素が１、各ｖ'番目の要素が−１／８、その他は０となる。
【００９５】
本発明の手法において、平滑化項ＣＳは、データ項ＣＤに対して次のような望ましい特性を持つ。前述のように、ステレオカメラが地面を斜め上から観測するように設置され、かつメッシュが規則性を持つ場合、カメラから近傍にある空間中の三角パッチは画像上では大きく見え、遠方にある三角パッチは画像上では小さく見える。すなわち、三角パッチの姿勢推定において、データ項ＣＤを通じて推定に寄与する画素は、カメラから近傍にある三角パッチでは多く、遠方にあるパッチでは少ない。一方、平滑化項ＣＳは、空間中のメッシュ形状に対して一様に作用するものであり、三角パッチの位置によらず一定である。したがって、データ項ＣＤと平滑化項ＣＳを同時に利用したとき、カメラ近傍の三角パッチの姿勢推定では、データ項ＣＤの寄与が相対的に大きくなり、遠方の三角パッチでは、逆に平滑化項ＣＳの寄与が相対的に大きくなる。
【００９６】
移動ロボットでは、近傍は遠方よりも精度の高いサーフェスが必要である。そのような領域に対しては、データ項ＣＤにより、画像からの観測データを重視したサーフェス生成を行うことができる。これに対し、精度を重視しない遠方では、精度よりも安定性を重視した推定を行うことができる。
（更新ノルム拘束項ＣＵ）
本発明の手法では、上述の二つの項に加え、以下の更新ノルム拘束項ＣＵを利用する。
【００９７】
【数３０】
2014228941.tif 000032
【００９８】
対角要素のみに値ｄｖ≧０を持つ。この項は、繰返し毎の
【００９９】
【数３１】
2014228941.tif 000033
【０１００】
極端に大きな変化を生じさせないように働く。
【０１０１】
本発明の手法では、前述のように、遠方の三角パッチでは、平滑化項ＣＳが推定に大きく寄与し、データ項ＣＤの影響は小さい。よって、遠方のサーフェスは、ＣＳを小さくしようとして平面に近い状態となる。しかし、遠方のサーフェスが平面に近い状態であれば、
【０１０２】
【数３２】
2014228941.tif 000034
【０１０３】
どんな値でもＣＳは小さいため、遠方のサーフェスが平面に近い状態のまま、繰返し計算毎に
【０１０４】
【数３３】
2014228941.tif 000035
【０１０５】
大きく変化することが起こりうる。更新ノルム拘束項ＣＵは、上述のような変動を抑制するために付与するものである。
【０１０６】
更新ノルム項は、最適化における収束を遅くする可能性がある。ただし、本発明の手法では、後述する階層的メッシュを利用することにより、十分な収束の速さを確保できる。また、
【０１０７】
【数３４】
2014228941.tif 000036
【０１０８】
カメラから遠方のパッチであるほど大きな値を持つように定めることが可能であるが、平滑化項の場合と同様に、カメラから近傍にある三角パッチは、遠方のパッチよりもデータ項が相対的に強く寄与しているため、実用上は単位行列でも特に問題はない。
（最適化アルゴリズム）
最適化では、前述のように、繰返し計算毎に
【０１０９】
【数３５】
2014228941.tif 000037
【０１１０】
と更新する。そして、この処理を所定の収束条件を満たすまで繰返す。以下では、式（１）の
【０１１１】
【数３６】
2014228941.tif 000038
【０１１２】
Gauss-Newton法に基づいて、導出するとともに、階層的メッシュによる推定、画素使用判定の方法についても述べる。
【０１１３】
【数３７】
2014228941.tif 000039
【０１１４】
式（１）を微分してゼロとおくことで、
【０１１５】
【数３８】
2014228941.tif 000040
【０１１６】
ただし、データ項ＣＳは非凸関数なので、Gauss−Newton法に基づいてこれを近似する。得られる
【０１１７】
【数３９】
2014228941.tif 000041
【０１１８】
次のように書ける。
【０１１９】
【数４０】
2014228941.tif 000042
【０１２０】
ＣＤ，ＣＳ，ＣＵから導かれる近似ヘッセ行列をそれぞれ表わし、
【０１２１】
【数４１】
2014228941.tif 000043
【０１２２】
ＣＤ，ＣＳから導かれる最急降下勾配をそれぞれ表わす。なお更新ノルム拘束項ＣＵは、
【０１２３】
【数４２】
2014228941.tif 000044
【０１２４】
二次形式であるため、右辺には影響を与えない。以下ではこれらのヘッセ行列と最急降下勾配を導出する。
【０１２５】
【数４３】
2014228941.tif 000045
【０１２６】
式（５）のデータ項に対し、
【０１２７】
【数４４】
2014228941.tif 000046
【０１２８】
微分しゼロとおくと次式を得る。
【０１２９】
【数４５】
2014228941.tif 000047
【０１３０】
微分の連鎖法則により次のように書ける。
【０１３１】
【数４６】
2014228941.tif 000048
【０１３２】
式（４）で示した平面射影変換行列を
【０１３３】
【数４７】
2014228941.tif 000049
【０１３４】
と定式化した際に、
【０１３５】
【数４８】
2014228941.tif 000050
【０１３６】
ラスタ順に並べたベクトルを示す。
【０１３７】
【数４９】
2014228941.tif 000051
【０１３８】
非特許文献２において、
【０１３９】
【数５０】
2014228941.tif 000052
【０１４０】
ベクトル値関数として導出されている。本発明の手法では、式（４）で示したように、ｐ番パッチのカメラ座標系における平面姿勢パラメータが
【０１４１】
【数５１】
2014228941.tif 000053
【０１４２】
ベクトル値関数で表わされるため、これを利用することにより、式（１３）の他のヤコビアンが導出できる。
【０１４３】
【数５２】
2014228941.tif 000054
【０１４４】
依存する。しかし、非特許文献１に示されているように、
【０１４５】
【数５３】
2014228941.tif 000055
【０１４６】
依存しないため、繰返し処理を行う前に、基準画像上の各画素について一度だけ計算しておけばよい。一方で、ＩＣＩＡの考え方を利用せずに式（２）をそのまま利用した場合には、画素毎に異なる
【０１４７】
【数５４】
2014228941.tif 000056
【０１４８】
を繰返し毎に算出しなければならず、計算コストが非常に大きい。すなわち、式（２）の場合と比較して、式（５）を利用することにより、大幅に効率的なサーフェス生成を実現できる。
【０１４９】
【数５５】
2014228941.tif 000057
【０１５０】
繰返し毎に変化する。ただし、これらはパッチごとに計算すればよく、画像座標毎に計算する必要はない。さらに、このうち
【０１５１】
【数５６】
2014228941.tif 000058
【０１５２】
依存した固定値となる。
【０１５３】
以上のことを考慮すると、
【０１５４】
【数５７】
2014228941.tif 000059
【０１５５】
次式のように書ける。
【０１５６】
【数５８】
2014228941.tif 000060
【０１５７】
ただし、κｐは、
【０１５８】
【数５９】
2014228941.tif 000061
【０１５９】
のスケール値である。また、ここでは、式（１４）（１５）において、
【０１６０】
【数６０】
2014228941.tif 000062
【０１６１】
による画素毎の加算の中に
【０１６２】
【数６１】
2014228941.tif 000063
【０１６３】
サイズが１×９となるのに対し、
【０１６４】
【数６２】
2014228941.tif 000064
【０１６５】
のサイズが１×３であるため、繰返し毎に多数の画素に対して必要な加算処理の計算コストが小さくできるからである。このとき、
【０１６６】
【数６３】
2014228941.tif 000065
【０１６７】
で決まるため、画像入力前にその積を算出しておくことができ、推定時には計算不要である。
【０１６８】
【数６４】
2014228941.tif 000066
【０１６９】
式（７）は
【０１７０】
【数６５】
2014228941.tif 000067
【０１７１】
ゼロとおくことで次式を得る。
【０１７２】
【数６６】
2014228941.tif 000068
【０１７３】
固定である。また、
【０１７４】
【数６７】
2014228941.tif 000069
【０１７５】
データ項のものに比べ、計算コストは非常に小さい。
【０１７６】
【数６８】
2014228941.tif 000070
【０１７７】
同様に、式（９）を
【０１７８】
【数６９】
2014228941.tif 000071
【０１７９】
ゼロとおくことから次式を得る。
【０１８０】
【数７０】
2014228941.tif 000072
【０１８１】
平滑化項の場合と同様に、
【０１８２】
【数７１】
2014228941.tif 000073
【０１８３】
固定となる。
（階層的メッシュによる推定）
本発明の手法では、地上座標系ｘ−ｙ平面上に描いたメッシュが細かいほど、任意の３次元形状を精度よく近似できる。しかし、メッシュが細かい場合は、頂点数が多くなり、
【０１８４】
【数７２】
2014228941.tif 000074
【０１８５】
の次元が大きくなるため、初期値と真値の差が大きい場合には、局所解に陥りやすい。
【０１８６】
そこで、本発明の手法では、より安定的なサーフェス生成を行うため、メッシュを階層的に細かくしてゆくアプローチを用いる。すなわち、最初は粗いメッシュを用いてサーフェスを推定し、その結果を初期値として、より細かいメッシュを用いたサーフェス生成を順次行う。このアプローチは、最初は粗いメッシュを用いてラフなサーフェスを生成し、徐々に詳細なサーフェスを求めてゆくことを意味し、アプリケーションが必要とするサーフェスの細かさに応じて、メッシュの粗さを制御できるという利点もある。
【０１８７】
階層的メッシュによる推定は、更新ノルム拘束項ＣＵによる収束遅延を低減する働きも持つ。一般に、
【０１８８】
【数７３】
2014228941.tif 000075
【０１８９】
小さな粗いメッシュでは、
【０１９０】
【数７４】
2014228941.tif 000076
【０１９１】
の値は小さくなり、一方、細かいメッシュでは、
【０１９２】
【数７５】
2014228941.tif 000077
【０１９３】
は大きくなる。これは、細かいメッシュほど強いノルム拘束が課されることを意味する。これにより、初期値と真値の差が大きい粗いメッシュの段階では、ノルム拘束の働きは弱く、データ項ＣＤを重視した推定が行われるのに対し、ラフなサーフェス形状が与えられ、部分的に細かい形状を推定する細かいメッシュの段階では、ノルム拘束項ＣＵが相対的に強く働き、カメラからの遠方のサーフェスに大きな変動を生じさせないような推定が行われる。  
（画素の使用判定）
サーフェス形状によってはオクルージョンが発生するため、上述の繰返し処理をそのまま適用すると、適切なサーフェスの生成が行えないことがある。そこで、本発明の方法では、繰返し計算毎に三角パッチを画像に投影した際に、その投影三角形に含まれる画素についての使用判定を行い、有効な画素のみに対しデータ項ＣＤを処理する。
【０１９４】
画素の使用／不使用は、次の３つのチェックの論理積で判定することができる。なお、三角パッチは全て両方のカメラの前の領域（ｚ０＞０∩ｚ１＞０）にあるものとする。
（１）画像内チェック：空間中のｐ番パッチを基準画像Ｔと参照画像Ｉそれぞれに投影し、投影三角形内の画素が、基準・参照のいずれかで画像外となれば
【０１９５】
【数７６】
2014228941.tif 000078
【０１９６】
（２）不可視パッチチェック：空間中のｐ番パッチの法線と、パッチの任意の一頂点から基準カメラ及び参照カメラの光学中心へ向かう逆視線とのなす角を算出する。二本の逆視線のいずれか一方のなす角が、９０度以上となれば、その投影三角形に含まれる全ての画素に対し、
【０１９７】
【数７７】
2014228941.tif 000079
【０１９８】
（３）オクルージョンチェック：空間中の各三角パッチを基準画像及び参照画像に投影した際に、投影三角形内の画素について、それぞれの画素におけるカメラからの奥行きを算出できる。それぞれの画像において、一個のパッチのみが割り当てた画素については、そのパッチに対して
【０１９９】
【数７８】
2014228941.tif 000080
【０２００】
二つ以上のパッチが割り当てられた画素については、奥行きが最も小さいパッチに
【０２０１】
【数７９】
2014228941.tif 000081
【０２０２】
（プログラムフロー）
本発明によるサーフェス生成では、画像が入力される前に、地上座標系のメッシュとカメラの内部・外部パラメータなどから式（１１）及び（１２）の計算に利用する
【０２０３】
【数８０】
2014228941.tif 000082
【０２０４】
基準画像上の画素毎について算出しておくことができる。また、平滑化拘束と更新ノルム拘束項に対応した
【０２０５】
【数８１】
2014228941.tif 000083
【０２０６】
の算出も可能であるため、これらを画像入力前に計算しておく。この部分をプレパレーションと呼ぶこととする。階層的メッシュを利用する場合には、階層数の分だけ
【０２０７】
【数８２】
2014228941.tif 000084
【０２０８】
算出しておけばよい。
【０２０９】
ステレオ画像が入力されたら、プレコンピューテーションにおいて、基準画像上の各要素について１×２の
【０２１０】
【数８３】
2014228941.tif 000085
【０２１１】
算出し、各画素について、
【０２１２】
【数８４】
2014228941.tif 000086
【０２１３】
後者の３×３の行列は、後の繰返し毎の計算において、各三角パッチが画像に投影された際に、三角パッチ内の画素の分だけ加算され、
【０２１４】
【数８５】
2014228941.tif 000087
【０２１５】
繰り返し毎の計算を、イタレーティブプロセスと呼ぶこととする。ここでは、まず、平滑化拘束による
【０２１６】
【数８６】
2014228941.tif 000088
【０２１７】
次に各パッチ毎の処理に入る。
【０２１８】
各パッチ毎の処理では、まず、三角パッチの画像を投影して、投影三角形に含まれる基準画像上の画素の集合を得る。次いで、平面パラメータベクトルと平面射影変換パラメータを算出し、その平面が両方のカメラから見えているかをチェックする。そして、Ｖ×３の
【０２１９】
【数８７】
2014228941.tif 000089
【０２２０】
およびスカラー値κｐを算出してそれらを積算する。ただし、
【０２２１】
【数８８】
2014228941.tif 000090
【０２２２】
パッチを構成する３頂点のみが関わるので、Ｖ×３ではなく３×３として計算してよく、
【０２２３】
【数８９】
2014228941.tif 000091
【０２２４】
３×３の行列として計算できる。
【０２２５】
画素毎の処理では、投影三角形に含まれる基準画像内の各画素について、画像内に入っているか等を判断しつつ、プレコンピューテーションで計算した
【０２２６】
【数９０】
2014228941.tif 000092
【０２２７】
また、画像座標を他方画像に投影して差分をとることで
【０２２８】
【数９１】
2014228941.tif 000093
【０２２９】
全画素について処理を終えたら、
【０２３０】
【数９２】
2014228941.tif 000094
【０２３１】
パッチ毎の処理を終える。そして、
【０２３２】
【数９３】
2014228941.tif 000095
【０２３３】
式（１０）を解き、サーフェスパラメータベクトルの更新を行う。これらの処理を、所定の収束条件を満たすまで繰り返す。
【０２３４】
階層的メッシュの場合には、
【０２３５】
【数９４】
2014228941.tif 000096
【０２３６】
一段細かいメッシュへ反映させ、新しいメッシュについて、プレコンピューテーションの部分から処理を行えばよい。
【０２３７】
ステレオカメラ１３０が撮像した左右画像５０２，５０３から二次元メッシュ８２で表現された３次元サーフェスが生成されたので、この３次元サーフェスを用いて、路肩候補抽出部が路肩を抽出する。この詳細を、図５を用いて説明する。
【０２３８】
図５は、図２に示した路肩候補抽出部１０５における処理の詳細を示すフローチャートである。３Ｄサーフェス情報取得処理２０１においては、３Ｄサーフェス生成部１０４が生成した３Ｄサーフェス情報を取得する。取得する情報は、３Ｄサーフェスを構成する全ての二次元メッシュ８２のＩＤ番号と二次元メッシュ８２の３頂点８１の地上座標系９０における３次元座標値である。
【０２３９】
次に、各二次元メッシュ８２の法線算出処理２０２において、３Ｄサーフェス情報取得処理２０１で取得した全ての二次元メッシュ８２について、法線ベクトルを計算する。法線ベクトルの計算では、３Ｄサーフェス情報取得処理２０１で取得した二次元メッシュ８２の３頂点８１の３次元座標値を用いて、３頂点８１を通る平面方程式を求める。この平面の法線が、二次元メッシュ８２の法線となる。
【０２４０】
次に、傾きが大きい二次元メッシュ抽出処理２０３において、各二次元メッシュの法線算出処理２０２で計算した全ての二次元メッシュ８２の法線ベクトルについて、地上座標系９０で予め定めたｚ＝０の平面との角度を算出する。ここで、ｚ＝０の平面は、例えば、ダンプトラック８０を起動した位置における地平面であり、水平面からの傾斜がほとんどない面を選定した仮想面である。計算して得られた各二次元メッシュ８２とｚ＝０で定義される平面とがなす角度の中で、所定しきい値以上の値となる場合に、その二次元メッシュ８２を路肩候補とする。
【０２４１】
最後に、二次元メッシュ８２とｚ＝０の平面とのなす角が大であって傾きが大きく、二次元メッシュ抽出処理２０３で路肩候補に選定された二次元メッシュを、路肩候補二次元メッシュ登録処理２０４において路肩候補二次元メッシュとして登録する。その際、二次元メッシュ８２のＩＤおよび３頂点８１の地上座標系９０における３次元座標値、法線ベクトル、ｚ＝０の平面とのなす角度が登録保存される。
【０２４２】
路肩候補抽出部１０５において、路肩候補二次元メッシュ８２を登録したので、その候補メッシュ８２が真に路肩６１，６２であるか否かを判定するために、路肩候補連続性判定部１０６が、路肩候補二次元メッシュ８２の連続性を判定する。この路肩候補連続性判定部の処理を図６に示す。
【０２４３】
図６は、路肩候補連続性判定部１０６での処理を示すフローチャートである。路肩候補二次元メッシュ情報取得処理３０１において、路肩候補連続性判定部１０６は、路肩候補抽出部１０５が登録保存した全ての二次元メッシュ８２のＩＤと３頂点８１の地上座標系９０における３次元座標値を取得する。
【０２４４】
次に、二次元メッシュ８２の位置情報によるグルーピング処理３０２において、路肩候補連続性判定部１０６は、路肩候補二次元メッシュ情報取得処理３０１で取得した路肩候補の全ての二次元メッシュ８２の中心座標を比較する。そして、隣り合う２つのメッシュ８２同士が、予め定めた距離よりも近い場合には、その２つのメッシュ８２を同一グループとする。次に、一方のメッシュ８２と、このメッシュ８２に隣り合う別のメッシュ８２の中心座標を比較する。これら２個のメッシュ８２，８２間の距離が所定距離より近い場合には、この別のメッシュ８２も同一グループに加える。この操作を繰り返すことにより、連続したメッシュ８２からなるグループが形成される。
【０２４５】
ここで、二次元メッシュ８２の中心座標として、この二次元メッシュ８２について取得した３頂点８１の重心を用いる。またメッシュ８２をグルーピングする際には、２個の二次元メッシュ８２の中心座標のユークリッド距離が所定の閾値以下であれば、同一グループとして同一グループＩＤを割り当てる。
【０２４６】
次に、路肩候補選別処理３０３において、上記グルーピング化によりグルーピング化されて得られた１個または複数の二次元メッシュ８２のグループの中から、路肩候補となるグループを選別する。その基準は、同一グループＩＤを有することにより同一グループに属すると判断されたグループにおいて、二次元メッシュ８２の数が所定閾値以上あれば、路肩候補のグループとする。選別されたグループは、路肩候補のグループとして登録される。
【０２４７】
最後に、路肩候補選別処理３０３で抽出した路肩候補のグループのグループＩＤおよびそのグループＩＤが割り当てられた同一グループに属する全ての二次元メッシュ８２のＩＤ、３頂点８１の地上座標系９０における３次元座標値、法線ベクトル、ｚ＝０の平面となす角度を、路肩候補二次元メッシュグループ登録処理３０４において、登録保存する。
【０２４８】
路肩候補のグループが選別されたので、図２に示した外部情報利用判定部１０７が、外部情報に基づいて、選定された路肩候補が真に路肩６１，６２であるか否かを判定する。この処理を、図７に示す。
【０２４９】
図７は、図２に示した外部情報利用判定部１０７の処理のフローチャートである。自車位置取得処理４０１では、図２に示した自車位置計測装置１１２から自車８０の現在位置の緯度経度情報を取得する。自車位置計測装置１１２は、ＧＰＳ(グローバル・ポジショニング・システム)等の情報を用いて、自車８０の位置を特定する。
【０２５０】
図２に示したように、計測演算装置１４０には地図データ１０８を記憶する記憶手段が付設されている。自車位置周辺地図情報取得処理４０２では、外部情報利用判定部１０７が上記地図データ１０８を参照して、自車位置取得処理４０１で取得した緯度経度に相当する部分の地図データ１０８を、所定範囲で取得する。
【０２５１】
さらに外部情報利用判定部１０７は、交差点の有無を判定する。この交差点有無判定処理では、自車位置計測装置１１２が有する記憶手段に記憶した走行可能領域６０の緯度経度情報を利用する。この場合の走行可能領域６０の緯度経度情報は、自車８０がこれまで辿ってきた走行可能領域６０の緯度経度情報であり、例えば５分間の情報を絶えず書き換えたもので、上記記憶手段に時系列的に記憶されている。時系列的な緯度経度情報と、自車８０の前方に広がる走行可能領域６０であって自車８０から所定距離の範囲内の地図情報とから、交差点に相当する走行可能領域(道路)６０が存在するか否かを判定する。
【０２５２】
ここで交差点であるか否かの判定は、地図データ１０８に交差点の道路種別情報が付与されている場合は、その道路種別情報を利用する。交差点の道路種別情報が得られない場合には、３個以上のリンクを持つノードを交差点とする。ここで、地図データ１０８では、道路網を交差点または分岐点をノードとし、ノード間の道路をリンクとして表現している。
【０２５３】
交差点有無判定処理４０３において自車８０の前方に交差点有りと判定した場合には、路肩候補信頼性判定処理４０４において、路肩候補連続性判定部１０６が路肩候補として登録した二次元メッシュグループの信頼度を所定量だけ下げる。ここで信頼度とは、前述のように同一グループＩＤを有することにより同一グループに属すると判断されたグループにおいて、二次元メッシュ８２の数が所定閾値以上あれば、路肩候補のグループとしているが、この二次元メッシュの数が多いほど信頼度が高いとして算出したものである。
【０２５４】
次にステレオカメラ１００の距離算出部１０９の処理内容の詳細を、図８ないし図１０を用いて説明する。図８は、走行可能領域６０内にある物体を、ステレオカメラ１３０を用いて認識する様子を説明する模式図である。図９は、左右の撮像部１０１，１０２が撮像した撮像画面５０２，５０３から、上記物体を特定する方法を説明するための図である。図１０は、ステレオカメラ１３０の撮像原理を示す図であり、同図(ａ)は物体と撮像面との関係を示す図であり、同図(ｂ)は左右両撮像画面５０２，５０３内での物体位置を示す図である。
【０２５５】
左右撮像部１０１，１０２で撮像した画像５０２，５０３のデータを受信する。この処理は、並列処理でも交互処理でもよい。取得した左右２枚の画像５０２，５０３を比較して、同一物体を撮像している部分を特定する。すなわち図８に示すように、走行可能領域(走行路上)６０にある対象物の物体５０１を路肩検出装置１００で撮像すると、左右の撮像部１０１，１０２が撮像した画像５０２、５０３は、それぞれ左画像５０２、右画像５０３として、図８に模式的に示したように表現される。
【０２５６】
ここで、同一の物体５０１は、左画像５０２では位置５０４に撮像され、右画像５０３では位置５０５に撮像される。左右の画像５０２，５０３の視野には、画像の横方向にδ1のずれがあるので、左画像５０２の位置５０４に撮像されている物体が、右画像５０３ではどれだけ変位して撮像されているかを特定する必要がある。
【０２５７】
次に、この左右の画像で異なる位置に撮像された物体５０１の位置特定方法を、図９を用いて説明する。図９において、左画像５０２および右画像５０３の座標系を次のように定義する。横方向ｑａ軸６０１、縦方向をｑｂ軸６０２とする。左画像５０２内にハッチングして示した小さな矩形領域６０３を定め、この矩形領域６０３の各頂点を、ｑａ−ｑｂ座標系で、(ｑａ１，ｑｂ１)、(ｑａ１，ｑｂ２)、(ｑａ２，ｑｂ１)、(ｑａ２，ｑｂ２)と表わす。
【０２５８】
一方、右画像５０３において、左画像５０２の矩形領域６０３にその大きさが対応するハッチングで示した矩形領域６０５を定める。この矩形領域６０５の各頂点を、(Ｑａ，ｑｂ１)、(Ｑａ，ｑｂ２)、(Ｑａ＋（ｑａ２−ｑａ１），ｑｂ１)、(Ｑａ＋（ｑａ２−ｑａ１），ｑｂ２)と設定する。次に、Ｑａの値をｑａ＝０からｑａ＝ｑ３まで増加させる。これにより、図９で破線で囲った矩形領域６０４が、左から右へと走査される。
【０２５９】
この走査においては、左画像５０２内の矩形領域６０３における画像と、右画像５０３内の矩形領域６０４のおける画像の相関値を比較する。比較の結果、左画像５０２の矩形領域６０３と相関性が最も高かった右画像５０３の矩形領域６０５の位置(ｑａ４，ｑｂ１)、(ｑａ４，ｑｂ２)、(ｑａ４＋(ｑａ２−ｑａ１)，ｑｂ１)、(ｑａ４＋(ｑａ２−ｑａ１)，ｑｂ２)を、矩形領域６０３に撮像された物体と同一物体が撮像された右画像５０３内の領域とする。
【０２６０】
ここで、左画像５０２内の矩形領域６０３内の各画素と、右画像５０３内の矩形領域６０５内の各画素は、画素同士対応しているものとする。また、右画像５０３の矩形領域６０４を走査する際、相関値が予め定めた値以上になる矩形領域が見つけられなかった場合には、右画像５０３内には、左画像５０２の矩形領域６０３に対応する「対応点無し」と判断する。
【０２６１】
次に、左画像５０２内の矩形領域６０３をこの矩形領域６０３の大きさ分だけｑａ軸方向に移動させて、矩形領域６０６を設定する。この移動させた矩形領域６０６に対して、右画像５０３内で、上記と同様の走査処理を実行し、右画像５０３内に対応する矩形領域を見つける。さらに、左画像５０２内で矩形領域をｑａ軸方向に移動して右画像５０３内で同様の走査処理を実行し、左画像５０２のｑａ軸方向で右端まで達したら、ｑａ軸方向で左端まで戻り、ｑｂ軸方向に矩形領域分６０３だけ移動させる。この手順を繰り返し、左画像５０２内の全領域で矩形領域６０３を走査して、左画像５０２の全画素に対して、右画像５０３内の対応点を求める。対応点が見つからない場合は、「対応点無し」とする。
【０２６２】
走行路６０内にある物体５０１の左右画像５０２，５０３における位置が特定できたので、次に自車８０から物体５０１までの距離を算出する。つまり、同一物体５０１を撮像した左画像５０２と右画像５０３内の対応点が、走行可能領域検出装置１００からどの程度の距離の位置にあるかを算出する。
【０２６３】
図１０に、左撮像部１０１と右撮像部１０２が、同一点(対応点)７０１を検出したときの走行可能領域検出装置１００から対応点７０１までの距離を算出する原理図を示す。左撮像部１０１は、焦点距離ｆのレンズ７０２と撮像面７０３を備え、レンズ７０２の中心を通り撮像面７０３に垂直方向に延びる光軸７０８を有するカメラである。同様に、右撮像部１０２は、焦点距離ｆのレンズ７０４と撮像面７０５を備え、レンズ７０４の中心を通り撮像面７０５に垂直方向に延びる光軸７０９を有するカメラである。なお、左右の撮像部１０１，１０２はレンズ７０２，７０４の中心を基準にして、左右に距離δだけ離れて配設されている。
【０２６４】
２個の撮像部１０１，１０２の前方の対応点７０１は、左撮像部１０１の撮像面７０３上で点７０６（光軸７０８から距離δ２だけ離れた点）として撮像される。そしてこの点は、左画像５０２では点７０６（光軸７０８から画素δ４だけ離れた位置）として記録される。同様に、右撮像部１０２の撮像面７０５では、対応点７０１は点７０７（光軸７０９から距離δ３だけ離れた点）に撮像され、右画像５０３では点７０７（光軸７０９から画素δ５だけ離れた位置）として記録される。
【０２６５】
このように同一の点７０１が、左画像５２では光軸７０８から左へδ４画素離れた位置に、右画像５０３では光軸７０９から右へδ５画素離れた位置に撮像される。その結果、（δ４＋δ５）画素の視差が発生する。このため、左撮像部１０１の光軸７０８と対応点７０１との距離をξとすると、走行可能領域検出装置１００から対応点７０１までの距離Δを、次式により求めることができる。
【０２６６】
対応点７０１と左撮像部１０１との関係から  δ２：ｆ＝ξ：Δ
対応点７０１と右撮像部１０２との関係から  δ３：ｆ＝(δ−ξ)：Δ
従って、
Δ＝ｆ×δ／(δ２＋δ３)＝ｆ×δ／｛(δ４＋δ５)×ａ｝
となる。ここで、ａは左右撮像面７０３、７０５の撮像素子のサイズである。ｆ、δは既知であり、上述した走査により、δ４、δ５は求められるので、走行可能領域検出装置１００から対応点７０１までの距離Δが求められる。
【０２６７】
以上述べた対応点７０１までの距離の算出を、左右の画像５０２，５０３内に出現した全ての対応点７０１に関して実行する。その結果、走行可能領域検出装置１００から対象物５０１まで、換言すれば、自車８０から対象物５０１までの距離を表現した距離画像が得られる。得られた距離画像は、走行可能領域検出装置１００が備える図示しない表示手段に表示されるとともに、この走行可能領域検出装置１００が備える記憶手段に保存される。
【０２６８】
なお、走行可能領域６０に物体がある場合の距離算出処理は、割り込み処理により実行される。すなわち、左撮像部１０１及び右撮像部１０２から計測演算装置１４０に画像信号が割り込み入力された場合には、上記処理を実行する。左撮像部１０１及び右撮像部１０２から画像信号の割り込み入力がない場合には、画像入力がこの計測演算装置１４０に入力されるまで待機する。
【０２６９】
上記実施例では、物体の検出と距離算出を、１台のステレオカメラを用いて実行している。しかしながら、急カーブ等の検出が困難な場合を想定して、ステレオカメラを複数台建設機械に搭載するようにしてもよいし、または車両の前照灯のように画角自体を変更できるような構成にしてもよい。
【０２７０】
以上述べたように、本発明の実施例によれば、走行可能領域およびその左右両側に形成される路肩で構成される地面をメッシュ化したので、各メッシュの法線方向から走行可能領域かまたは路肩かの別を容易に判断できる。その際、繰返し計算のコストを低減するようにしたので、短時間に判断でき車両の実用的な走行速度に対応できる。また、路肩の不連続点から、交差点や分岐点を検出できるので、車両の自動運転も可能になる。さらに、走行可能領域に走行を妨げる物体があれば、事前にその物体を認識し、その物体までの距離を計測できるので、建設機械等の車両に回避指示を発生することができる。
【０２７１】
なお上記実施例は例示的なものであって、本発明はこの実施例に制限されるものではなく、本発明の範囲は添付した特許請求の範囲で示されるものである。
【符号の説明】
【０２７２】
６０…走行可能領域（走行路）、６１、６２…路肩、７０…三次元サーフェス(地面)、８１…頂点、８２…二次元メッシュ、８３…メッシュ、８５…地表面、９０…地球座標、９１、９２…カメラ座標系、１００…路肩検出装置(走行可能領域検出装置)、１０１…左撮像部、１０２…右撮像部、１０３…距離算出部、１０４…３Ｄサーフェス生成部、１０５…路肩候補抽出部、１０６…路肩候補連続性判定部、１０７…外部情報利用判定部、１０８…地図データ、１０９…路肩出力部、１１０…車両制御量作成部、１１２…自社位置計測装置、１２０…制御装置、１３０…撮像手段（ステレオカメラ）、１４０…計測演算装置、５０１…物体、５０２…左画像、５０３…右画像、５０４、５０５…撮像物体、６０１…左右画像のｑａ軸、６０２…左右画像のｑｂ軸、６０３〜６０６…矩形領域、７０１…対応点、７０２…レンズ、７０３…撮像面、７０４…レンズ、７０５…撮像面、７０６、７０７…撮像面の撮像位置、７０８、７０９…光軸、ｆ…焦点距離、δ…左右カメラの距離、δ１…視差、δ２、δ３…撮像面での距離、δ４、δ５…画像上での画素数、Δ…対応点までの距離、ξ…距離。
【図１】
2014228941.tif 000097
【図２】
2014228941.tif 000098
【図３】
2014228941.tif 000099
【図４】
2014228941.tif 000100
【図５】
2014228941.tif 000101
【図６】
2014228941.tif 000102
【図７】
2014228941.tif 000103
【図８】
2014228941.tif 000104
【図９】
2014228941.tif 000105
【図１０】
2014228941.tif 000106