JP5623362B2

JP5623362B2 - 段差部認識装置

Info

Publication number: JP5623362B2
Application number: JP2011213384A
Authority: JP
Inventors: 五十志奈良村
Original assignee: Honda Motor Co Ltd
Current assignee: Honda Motor Co Ltd
Priority date: 2011-09-28
Filing date: 2011-09-28
Publication date: 2014-11-12
Anticipated expiration: 2031-09-28
Also published as: JP2013072813A

Description

本発明は、移動体の移動環境に存在する階段等の段差部の空間的な配置位置を、移動体に搭載されたステレオカメラによって取得されるステレオ画像を用いて認識する装置に関する。

従来、例えば自律的に移動可能なロボットに階段の昇降動作を行なわせる場合に、該ロボットに階段を認識させる技術として、例えば特許文献１に見られる技術が提案されている。

この技術では、ロボットに搭載したステレオビジョンシステムによってロボットの外界の距離データを取得し、この距離データを基に、階段の踏面に相当する平面部を検出する。そして、検出した平面部を包含する多角形をSklanskyのアルゴリズムや、Melkmanのアルゴリズムによって求め、その求めた多角形から、階段のフロントエッジ、バックエッジ、幅、長さを特定する。

国際公開公報ＷＯ２００５／０８７４５２

ところで、２足移動ロボット等の脚式移動ロボットに階段等の段差部の昇降動作を行なわせる場合、ロボットの脚の足平部を、段差部の踏面の先端側エッジ（特許文献１で言うところのフロントエッジ）に載せて、該足平部の一部を段差部の踏面からはみ出させることが必要もしくは好ましい場合がある。

例えば、階段の踏面の奥行き寸法が、ロボットの足平部の大きさに比して狭い場合には、該足平部の全体を踏面に載せることが不可能な場合がある。あるいは、ロボットの足平部の全体を踏面に載せることが可能であっても、該足平部を振り出すときに、該足平部が階段の蹴り込み等と干渉するのを避けるために、ロボットの足平部の一部を踏面からはみ出させることが好ましい場合もある。

そして、このような場合には、ロボットの足平部の目標とする着地位置を適切に決定するために、階段等の段差部の先端側エッジの位置及びその方向を精度よく認識する必要がある。

しかるに、前記特許文献１に見られる如き従来の手法では、階段などの段差部の踏面の先端側エッジを検出することができない場合が多々ある。

すなわち、ステレオビジョンシステムによって得られるステレオ画像から認識される距離データは、撮像画像のテクスチャ等の影響を受けやすいので、段差部の踏面の先端側エッジ付近での信頼性の高い距離データを得ることができない場合も多々ある。そして、このような場合には、段差部の踏面の先端側エッジ付近は、平面部分として認識できないか、もしくは、平面部分から除外されることとなる。その結果、前記多角形は、段差部の踏面の先端側エッジと異なる境界の形状を有するものとなり、該多角形から認識されるフロントエッジは、段差部の踏面の実際の先端側エッジとの誤差が大きなものとなりやすい。

従って、前記特許文献１に見られる如き従来の手法では、階段などの段差部の踏面の先端側エッジを検出することができない場合が多々ある。

本発明は、かかる背景に鑑みてなされたものであり、ステレオカメラにより取得されるステレオ画像を利用して、階段等の段差部の踏面の先端側エッジの空間的な位置及び方向を含む該段差部の配置位置を精度良く認識することができる装置を提供することを目的とする。

ここで、本発明における用語について補足しておく。本発明においては、「段差部」は、下段側踏面とその一段上側の上段側踏面との組を一組以上（一組又は複数組）有するものである。この場合、「段差部」の各「踏面」は、ある仮想的な平面（無限平面）に包含される平坦な面とみなすことができるような面であるとする。そして、本発明における「段差部」は、その一形態として階段を含むものであるが、階段だけを意味するものではない。例えば、床面に設置される踏み台もしくは任意の設置物により構成される段差部も含まれる。

また、段差部の上段側踏面の「先端側エッジ」というのは、段差部をその下段側踏面が手前側、上段側踏面が奥側となる視線で見た場合において、上段側踏面の外形の境界線のうち、手前側で横方向（水平方向もしくはそれに近い方向）に直線状に延在する境界線を意味する。

換言すれば、段差部の上段側踏面の「先端側エッジ」というのは、段差部をその上段側踏面が手前側、下段側踏面が奥側となる視線で見た場合において、上段側踏面の外形の境界線のうち、奥側で横方向に直線状に延在する境界線を意味する。

以上を前提として、本発明を以下に説明する。

本発明の段差部認識装置は、上記の目的を達成するために、ステレオカメラを搭載した移動体の移動環境に存在しており、下段側踏面とその一段上側の上段側踏面との組を一組以上有する段差部の空間的な配置位置を、前記ステレオカメラにより取得されるステレオ画像を構成する基準画像及び参照画像を用いて認識する段差部認識装置であって、
前記段差部が前記ステレオカメラにより撮像されている状態において、前記基準画像から、該段差部の上段側踏面及び下段側踏面にそれぞれ対応する２つの平面部分を少なくとも含む互いに異なる平面部分がそれぞれ投影された領域である複数の平面投影領域を抽出すると共に、各平面投影領域に対応する平面部分を包含する平面の空間的な位置及び姿勢を表す平面パラメータを決定する平面投影領域抽出手段と、
前記段差部の上段側踏面の先端側エッジを前記基準画像に投影してなる実エッジ投影線をそれぞれ含み、且つ、前記基準画像の縦方向に細長い形状を有して該基準画像の横方向に並列する複数条の演算処理領域を該基準画像に設定すると共に、該複数条の演算処理領域のそれぞれにおける前記基準画像の縦方向での前記実エッジ投影線の複数の候補位置を設定し、各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を、前記基準画像及び参照画像と、前記決定された平面パラメータとを用いて算出する評価関数算出手段と、
前記実エッジ投影線を推定してなる線である推定エッジ投影線を、前記複数条の演算処理領域のそれぞれにおける該推定エッジ投影線の位置の適合度を合成してなる合成適合度が最も高くなるように、前記算出された評価関数に基づいて決定する推定エッジ投影線決定手段と、
該段差部の空間的な配置位置を認識するためのデータとして、少なくとも前記段差部の上段側踏面の先端側エッジの空間的な位置及び方向を表すデータを前記決定された推定エッジ投影線と前記決定された平面パラメータとに基づいて決定する段差部空間配置決定手段とを備え、
前記評価関数算出手段は、前記各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を算出するために、各演算処理領域内に各候補位置を境界として隣接する２つの射影変換対象領域を少なくとも含む複数のエッジ推定用射影変換対象領域を各候補位置に応じて設定する処理と、各エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータを前記平面投影領域抽出手段により決定された平面パラメータに応じて設定する処理と、各演算処理領域及び各候補位置の組毎に、各エッジ推定用射影変換対象領域の画像の画素値分布と、該エッジ推定用射影変換対象領域に対応する前記参照画像中の領域の画像を該エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す誤差関数の値を算出する処理と、各演算処理領域及び各候補位置の組毎に、各演算処理領域内に設定した複数のエッジ推定用射影変換対象領域のそれぞれに対応して算出した前記誤差関数の値を線形結合してなる値を、前記評価関数の値として算出する処理とを実行し、
前記推定エッジ投影線決定手段は、前記推定エッジ投影線の複数の候補として、前記基準画像での位置及び方向の一方又は両方が互いに異なる複数の推定エッジ投影線候補を設定する処理と、当該複数のエッジ投影線候補の各々に対して、前記複数条の演算処領域のそれぞれにおける該エッジ投影線候補の位置の適合度を表す値として前記算出された評価関数に基づいて決定した値を合成することにより各エッジ投影線候補の合成適合度を表す総合評価関数値を算出する処理と、前記複数のエッジ投影線候補のうち、当該総合評価関数値により表される合成適合度が最も高くなるエッジ投影線候補を前記推定エッジ投影線として決定する処理とを実行するように構成されていることを特徴とする（第１発明）。

かかる第１発明によれば、前記段差部が前記ステレオカメラにより撮像されている状態において、前記平面投影領域抽出手段の処理が実行される。この処理では、前記基準画像から、該段差部の上段側踏面及び下段側踏面にそれぞれ対応する２つの平面部分を少なくとも含む複数の平面投影領域が抽出される。さらに、各平面投影領域に対応する平面部分を包含する平面の空間的な位置及び姿勢を表す平面パラメータが決定される。

補足すると、第１発明における平面投影領域抽出手段の処理は、平面投影領域の抽出とそれに対応する平面パラメータの決定とを行い得るものであれば、どのような手法を用いるものであってもよい。その手法は、公知の手法であってもよい。例えば、移動体に搭載したレーザ測距装置等の公知の測距装置を用いて、段差部の各部の距離（移動体からの距離）を測定し、その測定データを基に、平面投影領域の抽出とそれに対応する平面パラメータの決定とを行うようにしてもよい。

あるいは、後述する射影変換（平面射影変換）を利用した手法によって、平面投影領域の抽出とそれに対応する平面パラメータの決定とを行なうことも可能である。

また、平面投影領域抽出手段により抽出する各平面投影領域は、それぞれに対応する平面部分（段差部の各踏面等）の全体の投影領域である必要はなく、該平面部分の一部分の投影領域であってもよい。例えば、段差部の上段側踏面に対応して抽出される平面投影領域には、該上段側踏面の先端側エッジを基準画像に投影した線が含まれている必要なない。

また、平面投影領域抽出手段により抽出する平面投影領域は、段差部の各踏面に対応する平面投影領域に限らず、例えば階段の蹴り込み面や階段の段鼻の先端面に対応する平面投影領域が含まれていてもよい。ただし、段差部の各踏面以外の平面部分に対応する平面投影領域を抽出対象から除外するようにしてもよい。その場合、例えば平面部分の法線方向の向きに基づいて、段差部の各踏面以外の平面部分に対応する平面投影領域を抽出対象から除外するようにすることが可能である。

第１発明では、次に、前記評価関数算出手段の処理が実行される。この処理では、評価関数算出手段は、前記段差部の上段側踏面の先端側エッジを前記基準画像に投影してなる実エッジ投影線をそれぞれ含み、且つ、前記基準画像の縦方向に細長い形状を有して該基準画像の横方向に並列する複数条の演算処理領域を該基準画像に設定する。

この場合、前記実エッジ投影線は、段差部の上段側踏面に対応する平面投影領域と、下段側踏面に対応する平面投影領域との間に存在するので、該実エッジ投影線をそれぞれ内部に含むような複数条の演算処理領域を、前記平面投影領域抽出手段の処理により得られた平面投影領域及び平面パラメータに基づいて設定することが可能である。なお、各演算処理領域は、基準画像の縦方向に細長い形状を有するので、基準画像の横方向での幅が微小なもの（例えば１画素分の幅）となるような領域である。

さらに、評価関数算出手段は、複数条の演算処理領域のそれぞれにおける前記基準画像の縦方向での前記実エッジ投影線の複数の候補位置を設定し、各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を、前記基準画像及び参照画像と、前記決定された平面パラメータとを用いて算出する。

この評価関数は次のように算出される。すなわち、評価関数算出手段は、各演算処理領域内に各候補位置を境界として隣接する２つの射影変換対象領域を少なくとも含む複数のエッジ推定用射影変換対象領域を各候補位置に応じて設定する。

この場合、各演算処理領域において、任意の１つの候補位置が、実エッジ投影線の位置に一致していると仮定すると、該候補位置を境界として隣接して設定される２つのエッジ推定用射影変換対象領域のうち、該候補位置の上側の領域は、段差部の上段側踏面の投影領域に相当するものとなる。また、候補位置の下側の領域は、段差部の上段側踏面の先端側エッジから下方に起立する面部（階段の段鼻又は蹴り込み面に対応する面部）の投影領域に相当するものとなる。

さらに、評価関数算出手段は、各エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータを前記平面投影領域抽出手段により決定された平面パラメータに応じて設定する。例えば、各候補位置を境界として隣接して設定される２つのエッジ推定用射影変換対象領域のうち、該候補位置の上側の領域に対応する射影変換用平面パラメータは、段差部の上段側踏面に対応して平面投影領域抽出手段により決定された平面パラメータに一致するものとされる。

また、各候補位置を境界として隣接して設定される２つのエッジ推定用射影変換対象領域のうち、該候補位置の下側の領域に対応する射影変換用平面パラメータは、例えば、段差部の上段側踏面と下段側踏面との間でこれらに対して起立する面に対応して平面投影領域抽出手段により決定された平面パラメータに一致するものとされる。

あるいは、当該射影変換用平面パラメータは、例えば段差部の上段側踏面に対してあらかじめ定められた角度（例えば９０degまたはそれに近い角度）で起立し、且つ、候補位置に対応する仮想的な先端側エッジ（該先端側エッジを基準画像に投影した線の位置（基準画像の縦方向での位置）が該候補位置に一致するような仮想的な先端側エッジ）の直線を含む平面の位置及び姿勢を表す平面パラメータ（これは上段側踏面に対応して平面投影領域抽出手段により決定された平面パラメータと候補位置とから算出することができる）に一致するものとされる。

さらに、評価関数算出手段は、各演算処理領域及び各候補位置の組毎に、各エッジ推定用射影変換対象領域の画像の画素値分布と、該エッジ推定用射影変換対象領域に対応する前記参照画像中の領域の画像を該エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す誤差関数の値を算出する。

なお、エッジ推定用射影変換対象領域に対応する参照画像中の領域というのは、基準画像のエッジ推定用射影変換対象領域に投影されている実空間の部分が該エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータにより規定される平面上の部分であると仮定して、該部分を参照画像中に投影した場合に得られる領域を意味する。

この場合、各演算処理領域において、ある候補位置が、実エッジ投影線の位置に一致しておれば、各エッジ推定用射影変換対象領域の画像の画素値分布と上記射影変換画像の画素値分布とは互いに一致（もしくはほぼ一致）する。一方、該候補位置が、実エッジ投影線の位置に一致しない場合には、各エッジ推定用射影変換対象領域の画像の画素値分布と上記射影変換画像の画素値分布とは互いに異なるものとなる。

そして、評価関数算出手段は、各演算処理領域及び各候補位置の組毎に、各演算処理領域内に設定した複数のエッジ推定用射影変換対象領域のそれぞれに対応して算出した前記誤差関数の値を線形結合してなる値を、前記評価関数の値として算出する。

これにより、基準画像の縦方向に細長い（基準画像の横方向での幅が微小な）各演算処理領域毎に、基準画像の縦方向での実エッジ投影線の候補位置と、該候補位置の実エッジ投影線の位置に対する適合度との相関関係を表すデータが得られることとなる。このデータは、換言すれば、各演算処理領域において、基準画像の縦方向でのどの位置が、実エッジ投影線の位置として妥当であるか（適合度が高いか）を示すものとなる。

次いで、推定エッジ投影線決定手段の処理が実行される。この処理では、推定エッジ投影線決定手段は、前記実エッジ投影線を推定してなる線である推定エッジ投影線を、前記複数条の演算処理領域のそれぞれにおける該推定エッジ投影線の位置の適合度を合成してなる合成適合度が最も高くなるように、前記算出された評価関数（各演算処理領域及び各候補位置の組毎に算出された評価関数の値）に基づいて決定する。

この場合、前記評価関数算出手段の処理によって、各演算処理領域及び各候補位置の組毎に算出された評価関数の値によって、各演算処理領域毎に、基準画像の縦方向での実エッジ投影線の候補位置と、上記評価関数の値との相関関係を表すデータが得られている。従って、ある推定エッジ投影線を仮定した場合、その推定エッジ投影線の、各演算処理領域における位置（基準画像の縦方向での位置）の、実エッジ投影線の実際に位置に対する適合度（妥当性の度合い）が判ることとなる。

そして、推定エッジ投影線が実エッジ投影線に一致していれば、その推定エッジ投影線の、各演算処理領域における位置の適合度は、原理的には、該演算処理領域において最も高くなる。

そこで、推定エッジ投影線決定手段は、前記複数条の演算処理領域のそれぞれにおける推定エッジ投影線の位置の適合度を合成してなる合成適合度が最も高くなるように該推定エッジ投影線を決定する。これにより、複数条の演算処理領域のそれぞれにおける実エッジ投影線に対する適合度が極力高いものとなるような推定エッジ投影線、すなわち、実エッジ投影線を推定したものとしての妥当性が高い推定エッジ投影線が決定される。
具体的には、前記推定エッジ投影線決定手段は、前記推定エッジ投影線の複数の候補として、前記基準画像での位置及び方向の一方又は両方が互いに異なる複数の推定エッジ投影線候補を設定する処理と、当該複数のエッジ投影線候補の各々に対して、前記複数条の演算処領域のそれぞれにおける該エッジ投影線候補の位置の適合度を表す値として前記算出された評価関数に基づいて決定した値を合成することにより各エッジ投影線候補の合成適合度を表す総合評価関数値を算出する処理と、前記複数のエッジ投影線候補のうち、当該総合評価関数値により表される合成適合度が最も高くなるエッジ投影線候補を前記推定エッジ投影線として決定する処理とを実行する。

なお、推定エッジ投影線決定手段の処理で決定する推定エッジ投影線は、その合成適合度が厳密に最も高いものである必要はなく、該推定エッジ投影線と基準画像での位置及び方向の一方又は両方が若干異なるような複数の線に比して、該合成適合度が相対的に高くなるようなものでよい。従って、基準画像での位置及び方向の一方又は両方が互いに異なる複数の推定エッジ投影線の候補の中で、最も、前記合成適合度が高いものを、推定エッジ投影線として決定してもよい。

次いで、前記段差部空間配置決定手段の処理が実行される。この処理では、段差部空間配置決定手段は、ステレオカメラの撮像領域に存在する段差部の空間的な配置位置を認識するためのデータとして、少なくとも該段差部の上段側踏面の先端側エッジの空間的な位置及び方向を表すデータを前記決定された推定エッジ投影線と前記決定された平面パラメータとに基づいて決定する。

この場合、段差部の上段側踏面の先端側エッジは、上段側踏面に対応して前記平面投影領域抽出手段により決定された平面パラメータにより空間的な位置及び姿勢が規定される平面上の線であり、この線を光学的に基準画像に投影してなる線が、上記推定エッジ投影線となる。従って、この推定エッジ投影線と、平面投影領域抽出手段により決定された平面パラメータ（上段側踏面に対応する平面部分の平面パラメータ）とに基づいて、先端側エッジの空間的な位置及び方向を表すデータを決定できることとなる。

該データとしては、例えば、先端側エッジ上の２つの点の空間的な位置、あるいは、先端側エッジ上の１つの点の空間的な位置と該先端側エッジの方向ベクトルとの組等を用いることができる。

以上説明した第１発明によれば、前記評価関数算出手段の処理によって、各演算処理領域及び各候補位置の組毎に算出される評価関数の値は、基準画像の縦方向に細長い各演算処理領域毎に、基準画像の縦方向での実エッジ投影線の候補位置と、該候補位置の実エッジ投影線の位置に対する適合度との相関関係を表すものとなる。

従って、この評価関数の値に基づいて、前記推定エッジ投影線を決定することにより、基準画像の横方向に並ぶ複数条の演算処理領域のそれぞれにおいて適合度が極力高くなるような推定エッジ投影線を決定できる。このため、実エッジ投影線に対する一致度合いの高い推定エッジ投影線を決定できることとなる。

よって、第１発明によれば、階段等の段差部の踏面の先端側エッジの空間的な位置及び方向を精度よく認識することができる。

上記第１発明においては、前記平面投影領域抽出手段は、レーザ測距装置等を使用することなく、前記ステレオ画像（基準画像及び参照画像）を用いて平面投影領域の抽出と平面パラメータの決定とを行なうようにすることも可能である。

具体的には、前記平面投影領域抽出手段は、前記基準画像における位置が互いに相違する複数の平面投影領域抽出用局所領域を該基準画像に設定する処理と、各平面投影領域抽出用局所領域の画像と該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである平面投影領域抽出用平面パラメータを、該平面投影領域抽出用局所領域の画像の画素値分布と、該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像を該平面投影領域抽出用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する処理とを実行し、各平面投影領域に含まれる前記平面投影領域抽出用局所領域のそれぞれに対応する平面投影領域抽出用平面パラメータにより規定される平面が互いに一致するように前記基準画像から各平面投影領域を抽出する（第２発明）。

この第２発明によれば、前記平面投影領域抽出手段は、前記基準画像における位置が互いに相違する複数の平面投影領域抽出用局所領域を該基準画像に設定する。該平面投影領域抽出用局所領域は、基準画像の全体に比して十分に小さい面積の領域である。

ここで、ある１つの平面投影領域抽出用局所領域の画像が平面部分の画像である場合には、その画像と、基準画像の該平面投影領域抽出用局所領域に投影されている実空間の部分（ここでは平面部分のうちの局所部分）を参照画像中に投影してなる部分の画像とは、当該平面部分を包含する平面の位置及び姿勢を表す平面パラメータに応じた射影変換（平面射影変換）によって対応付けられる。

すなわち、原理的には、平面投影領域抽出用局所領域に対応する参照画像中の画像に、該平面投影領域抽出用局所領域に投影されている平面部分の平面パラメータに応じた射影変換を施すことによって、基準画像の平面投影領域抽出用局所領域の画像と同じ画素値分布を有する画像が得られる。

一方、平面投影領域抽出用局所領域の画像が平面部分の画像であっても、実際の平面部分に対応する平面パラメータと異なる平面パラメータを使用して、平面投影領域抽出用局所領域に対応する参照画像中の画像に射影変換を施した場合に得られる画像は、基準画像の平面投影領域抽出用局所領域の画像と異なる画素値分布を有する画像となる。

そこで、第２発明では、平面投影領域抽出手段は、次に、各平面投影領域抽出用局所領域の画像と該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである平面投影領域抽出用平面パラメータを、該平面投影領域抽出用局所領域の画像の画素値分布と、該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像を該平面投影領域抽出用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する。

これにより、段差部の踏面等の平面部分が投影されている平面投影領域抽出用局所領域にあっては、その平面部分を包含する平面の位置及び姿勢を表す平面投影領域抽出用平面パラメータが適正に決定されることとなる。

なお、平面投影領域抽出用局所領域に単一の平面部分とそれ以外の部分とが含まれている場合には、決定される平面投影領域抽出用平面パラメータにより規定される平面は、一般には、該平面投影領域抽出用局所領域に含まれる平面部分を包含する平面とは異なるものとなる。

上記の如く各平面投影領域抽出用局所領域に対応する平面投影領域抽出用平面パラメータを決定した場合、同一の平面上に存在する平面投影領域抽出用局所領域に対応する平面投影領域抽出用平面パラメータは、互いに同一（もしくはほぼ同一）となる。

そこで、平面投影領域抽出手段は、次に、各平面投影領域に含まれる前記平面投影領域抽出用局所領域のそれぞれに対応する平面投影領域抽出用平面パラメータにより規定される平面が互いに一致するように前記基準画像から各平面投影領域を抽出する。なお、ここで、平面投影領域抽出用平面パラメータが互いに同一というのは、厳密に互いに同一であることだけを意味するものではなく、平面投影領域抽出用平面パラメータにより規定される平面の位置及び姿勢がある既定の範囲内でほぼ同一となるような場合も含まれる。

これにより、第２発明によれば、レーザ測距装置等の測距装置をステレオカメラとは別に移動体に搭載したりすることなく、ステレオ画像と射影変換（平面射影変換）とを利用した手法によって、基準画像から平面投影領域を抽出することができる。

前記第１発明又は第２発明においては、前記評価関数算出手段が各演算処理領域に設定する複数の候補位置は、全ての演算処理領域について共通のＮ3a個（Ｎ3a：２以上の整数）の候補位置であり、前記評価関数算出手段は、第ｋ1番目（ｋ1：１からＮ3aまでの整数のうちの任意の１つの整数）の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出するために、前記射影変換用平面パラメータが互いに同一となる各演算処理領域のエッジ推定用射影変換対象領域を合成してなる合成射影変換対象領域における画像と、該合成射影変換対象領域に対応する前記参照画像中の領域の画像を該合成射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる画像との誤差画像を求める処理を実行し、その誤差画像を用いて、第ｋ1番目の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出することが好ましい（第３発明）。

この第３発明によれば、前記評価関数算出手段は、前記複数の候補位置の任意の１つである第ｋ1番目の候補位置と各演算処理領域とに対応する前記誤差関数を算出する前に事前に、前記射影変換用平面パラメータが互いに同一となる各演算処理領域のエッジ推定用射影変換対象領域を合成してなる合成射影変換対象領域に係わる前記誤差画像を求めておく。そして、前記評価関数算出手段は、この誤差画像を用いて第ｋ1番目の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出する。これにより、各候補位置及び各演算処理領域の各組毎に個別に前記誤差関数の値を算出する場合に比して、各候補位置及び各演算処理領域の各組毎の誤差関数の値の算出を効率よく行なうことができる。

前記第１〜第３発明では、前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知する段差部存在検知手段をさらに備えている場合には、前記平面投影領域抽出手段、評価関数算出手段及び段差部配置決定手段の処理は、前記段差部存在検知手段によって前記ステレオカメラの撮像領域に段差部が存在していることが検知されることを必要条件として実行される処理とされる。

この場合、前記段差部存在検知手段は、外部から与えられる情報に基づいて段差部が存在しているか否かを検知するようにすることも可能であるが、前記基準画像及び参照画像を用いて当該検知を行なうこともできる。

具体的には、前記段差部存在検知手段は、前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知するための段差部存在検知用領域を前記基準画像に設定する処理と、該段差部存在検知用領域の画像と該段差部存在検知用領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである段差部存在検知用平面パラメータを、前記段差部存在検知用領域の画像の画素値分布と該段差部存在検知用領域に対応する領域の画像を該段差部存在検知用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する処理とを実行し、決定した該段差部存在検知用平面パラメータにより示される平面の法線方向の向きに基づいて前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知する（第４発明）。

この第４発明によれば、段差部存在検知手段の処理によって決定される前記段差部存在検知用平面パラメータにより規定される平面の法線方向の向きは、前記ステレオカメラの撮像領域に段差部が存在している場合には、該撮像領域が平坦な床面である場合の法線方向の向きよりも傾いたものとなる。このため、前記段差部存在検知用平面パラメータにより規定される平面の法線方向の向きに基づいて、前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知することができる。

前記第１〜第４発明においては、前記段差部が前記ステレオカメラにより撮像されている状態において、該段差部が昇りの段差部であるか、降りの段差部であるかを判断する段差部種別判断手段をさらに備えており、
前記評価関数算出手段は、
前記段差部種別判断手段によって前記段差部が昇り段差部であると判断された場合には、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が該段差部の上段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が該段差部の下段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定し、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第１及び第２のエッジ推定用射影変換対象領域を該演算処理領域の上部側に設定すると共に、該第１及び第２のエッジ推定用射影変換対象領域のうちの下側の領域である第２のエッジ推定用射影変換対象領域との間に該候補位置に応じた間隔を有する第３のエッジ推定用射影変換対象領域を該演算処理領域の下部側に設定し、
前記段差部種別判断手段によって前記段差部が降り段差部であると判断された場合には、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が下段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が上段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定し、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第４及び第５のエッジ推定用射影変換対象領域を該演算処理領域内に設定することが好ましい（第５発明）。

この第５発明によれば、前記段差部種別判断手段によって前記段差部が昇り段差部であると判断された場合にあっては、前記評価関数算出手段は、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が該段差部の上段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が該段差部の下段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定する。これにより、前記複数条の演算処理領域のそれぞれに、前記実エッジ投影線が含まれるように該複数条の演算処理領域を基準画像に設定することができる。

さらに、前記評価関数算出手段は、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第１及び第２のエッジ推定用射影変換対象領域を該演算処理領域の上部側に設定すると共に、該第１及び第２のエッジ推定用射影変換対象領域のうちの下側の領域である第２のエッジ推定用射影変換対象領域との間に該候補位置に応じた間隔を有する第３のエッジ推定用射影変換対象領域を該演算処理領域の下部側に設定する。

この場合、各演算処理領域及び各候補位置の任意の１つの組において、該演算処理領域に設定される第１〜第３のエッジ推定用射影変換対象領域は、それぞれ、当該候補位置が、実エッジ投影線の位置に一致する場合に、段差部の上段側踏面が投影される領域、該上段側踏面に対して起立する面（段鼻の先端面や蹴り込み面）が投影される領域、段差部の下段側踏面が投影される領域に相当するものとして設定される領域である。そして、これらの第１〜第３のエッジ推定用射影変換対象領域は、候補位置に応じて変化することとなる。

従って、ステレオカメラの撮像領域に昇りの段差部が存在する場合には、各演算処理領域及び各候補位置の各組に対応して前記評価関数算出手段により算出される評価関数の値には、該候補位置で隣接する第１及び第２のエッジ推定用射影変換対象領域の画像に加えて、これらと平面パラメータベクトルが異なる第３のエッジ推定用射影変換対象領域の画像の画素値が反映されることとなる。そのため、各演算処理領域の各候補位置の、実エッジ投影線の位置に対する適合度を表すものとしての前記評価関数の値の信頼性を高めることができる。

一方、前記段差部種別判断手段によって前記段差部が降り段差部であると判断された場合にあっては、前記評価関数算出手段は、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が下段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が上段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定する。

これにより、前記複数条の演算処理領域のそれぞれに、前記実エッジ投影線が含まれるように該複数条の演算処理領域を基準画像に設定することができる。

さらに、前記評価関数算出手段は、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第４及び第５のエッジ推定用射影変換対象領域を該演算処理領域内に設定する。

ここで、ステレオカメラの撮像流域に存在する段差部が降りの段差部である場合には、基準画像には、段鼻の先端面や蹴り込み面のように、上段側透明に対して起立する姿勢の面は写らない。従って、評価関数算出手段は、上記第４及び第５のエッジ推定用射影変換対象領域を演算処理領域内に設定する。これらの第４及び第５のエッジ推定用射影変換対象領域は、それぞれ、当該候補位置が、実エッジ投影線の位置に一致する場合に、段差部の上段側踏面が投影される領域、段差部の下段側踏面が投影される領域に相当するものとして設定される領域である。そして、これらの第４及び第５のエッジ推定用射影変換対象領域は、候補位置に応じて変化することとなる。

従って、ステレオカメラの撮像領域に存在する段差部が降りの段差部である場合には、各演算処理領域及び各候補位置の各組に対応して前記評価関数算出手段により算出される評価関数の値には、該候補位置で隣接する第４及び第５のエッジ推定用射影変換対象領域の画像の画素値が適切に反映されることとなる。そのため、各演算処理領域の各候補位置の、実エッジ投影線の位置に対する適合度を表すものとしての前記評価関数の値の信頼性を適切に確保することとができる。

よって、第５発明によれば、ステレオカメラの撮像領域に存在する段差部が昇りの段差部と降りの段差部とのといずれであっても、実エッジ投影線に相当するものとして推定エッジ投影線を適切な信頼性で決定することができる。

上記第５発明においては、前記ステレオカメラの撮像領域に存在する段差部が昇りの段差部である場合において、前記平面投影領域抽出手段により、段鼻の先端面や蹴り込み面のように上段側踏面に対して起立した姿勢の平面部分の平面投影領域を抽出するようにした場合には、前記第２のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータとして、当該起立した姿勢の平面部分の平面投影領域に対応する平面パラメータを使用することができる。

ただし、段差部の段鼻の先端面や蹴り込み面のように上段側踏面に対して起立した姿勢の平面部分は、一般に、上段側踏面に対して一般に垂直もしくはそれに近い角度で起立する。そこで、前記第５発明においては、前記評価関数算出手段は、前記段差部種別判断手段によって前記段差部が昇りの段差部であると判断された場合に、各エッジ推定用射影変換対象領域に対応するエッジ推定用平面パラメータを設定する処理において、前記第１のエッジ推定用射影変換対象領域と前記第３のエッジ推定用射影変換対象領域とのそれぞれに対応する射影変換用平面パラメータとして、それぞれ、前記段差部の上段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータと、前記段差部の下段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータとを設定し、前記第２のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータとして、第１のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータにより規定される平面に対してあらかじめ定めた角度（例えば９０deg又はそれに近い角度）で交差する平面の位置及び姿勢を表す平面パラメータを設定するようにしてもよい（第６発明）。

この第６発明によれば、前記ステレオカメラの撮像領域に存在する段差部が昇りの段差部である場合に、前記平面領域抽出手段により、各踏面に対応する平面投影領域だけを抽出した場合であっても、第１〜第３のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータを適切に設定することができる。

なお、前記段差部種別判断手段によって前記段差部が降りの段差部であると判断された場合にあっては、前記評価関数算出手段は、各エッジ推定用射影変換対象領域に対応するエッジ推定用平面パラメータを設定する処理において、前記第４のエッジ推定用射影変換対象領域と前記第５のエッジ推定用射影変換対象領域とのそれぞれに対応する射影変換用平面パラメータとして、それぞれ、前記段差部の下段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータと、前記段差部の上段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータとを設定するようにすればよい。

補足すると、前記第５発明又は第６発明において、前記第４発明の構成を組み込んだ場合には、前記段差部種別判断手段は、前記段差部存在検知手段により決定された段差部存在検知用平面パラメータにより示される平面の法線方向の向きに基づいて、前記段差部が昇りの段差部であるか、降りの段差部であるかを判断することができる。

これは、前記段差部が昇りの段差部であるか、降りの段差部であるかに依存して、前記段差部存在検知用平面パラメータにより示される平面の法線が、段差部が存在しない平坦な面の法線方向に対して傾く向きが異なるからである。

このように前記段差部種別判断手段を構成することで、段差部存在検知手段により決定された段差部存在検知用平面パラメータを利用して容易に、前記段差部が昇りの段差部であるか、降りの段差部であるかを判断することができる。

本発明においては、さらに、前記段差部空間配置決定手段は、前記段差部の上段側踏面の先端側エッジを、該上段側踏面に対応する前記平面パラメータにより規定される平面に対してあらかじめ定めた角度で交差する方向で該段差部の下段側踏面に対応する前記平面パラメータにより規定される平面に投影した線を、該下段側踏面の基端側の境界線として決定する手段を含むことが好ましい（第７発明）。

この第８発明によれば、前記段差部の上段側踏面の先端側エッジの空間的な位置及び方向を表すデータに加えて、前記段差部空間配置決定手段により下段側踏面の基端側の境界線が決定される。このため、上段側踏面の一段下側の下段側踏面の奥行きを認識できることとなる。

さらに本発明においては、前記移動体には、前記ステレオカメラのよる撮像時に該ステレオカメラの撮像領域にテクスチャを付与する投光器が搭載されていることが好ましい（第８発明）。

この第８発明によれば、ある平面部分が投影されている基準画像の任意の領域の画像の画素値分布と、それに対応する参照画像の領域の画像を、適宜の平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との差は、該平面パラメータが、実際の平面部分の平面パラメータに対して相違する場合に顕著なものとなりやすい。

このため、前記評価関数算出手段により算出される評価関数の値、あるいは、前記第２発明において平面投影領域抽出手段が決定する平面投影領域抽出用平面パラメータ（ひいては各平面投影領域の平面パラメータ）、あるいは、前記第４発明における段差部存在検知手段が決定する段差部存在検知用平面パラメータの信頼性を高めることができる。

図１（ａ），（ｂ）はそれぞれ、本発明の一実施形態におけるロボット（移動体）が階段を昇ろうとしている状況、降ろうとしている状況を示す斜視図。実施形態のロボットに搭載した投光器によってステレオカメラの撮像領域に付与されるテクスチャを例示する図。代表的な階段の構造を説明するための図。図１に示すロボットに備えた演算処理装置の機能的構成を示すブロック図。図４に示す段差部存在検知部の処理を示すフローチャート。図５のＳＴＥＰ１の処理を説明するための図。図７（ａ），（ｂ）は、図５のＳＴＥＰ２の処理を説明するための図。図４に示す平面投影領域抽出部の処理を示すフローチャート。図８のＳＴＥＰ１１，１５の処理を説明するための図。図８のＳＴＥＰ１３の処理により得られるデータを示すグラフ。図４に示すエッジ投影線推定処理部の処理を示すフローチャート。図４に示すエッジ投影線推定処理部の処理を示すフローチャート。図１１のＳＴＥＰ２２の処理を説明するための図。図１１のＳＴＥＰ２３の処理を説明するための図。図１１のＳＴＥＰ３０の処理を説明するための図。図１６（ａ），（ｂ）は、図１１のＳＴＥＰ３２の処理により得られるデータを例示するグラフ。図１１のＳＴＥＰ３５の処理を説明するための図。図１１のＳＴＥＰ３６の処理を説明するための図。図１２のＳＴＥＰ３７の処理を説明するための図。図１２のＳＴＥＰ３８の処理を説明するための図。図１２のＳＴＥＰ４５の処理を説明するための図。

本発明の一実施形態を図１〜図２０を参照して以下に説明する。

図１（ａ），（ｂ）を参照して、本実施形態では、移動体としての脚式移動ロボット１によって階段等の段差部の空間的な配置位置を認識する場合を例にとって説明する。図示例の脚式移動ロボット１（以下、単にロボット１という）は、左右一対の２つの脚体を備える２足歩行ロボットである。

このロボット１には、ステレオカメラを構成する左右一対のカメラ３Ｒ，３Ｌと、カメラ３Ｒ，３Ｌの撮像領域に投光する投光器４とが搭載されている。

カメラ３Ｒは、撮像領域に向って右側のカメラ、カメラ３Ｌは、撮像領域に向って左側のカメラであり、それぞれＣＣＤカメラ等により構成されている。なお、カメラ３Ｒ，３Ｌの撮像画像は、モノトーン画像及びカラー画像のいずれの画像であってもよい。また、図１（ａ），（ｂ）では、カメラ３Ｒ，３Ｌは、ロボット１の頭部に搭載されているが、ロボット１の胸部等、他の部位に搭載されていてもよい。

ロボット１は、これらのカメラ３Ｒ，３Ｌによって、ロボット１の周辺の移動環境（主に、ロボット１の進行方向前方の環境）を撮像することにより、カメラ３Ｒの撮像画像とカメラ３Ｌの撮像画像とから構成されるステレオ画像を随時、取得することが可能となっている。

また、本実施形態では、ロボット１は、カメラ３Ｒ，３Ｌによる撮像時に、その撮像領域にテクスチャを付与するための補助光ＳＬを投光器４から投光する。この投光器４は、本実施形態では、図２に示す如く、撮像領域の面（図２では平面）に、互いに平行に並ぶ複数の斜行輝線ＳＬａと、この斜行輝線ＳＬａに対して交差する方向で互いに平行に並ぶ複数の斜行輝線ＳＬｂとから構成されるテクスチャを形成する所定色（例えば赤色）のレーザ光を上記補助光ＳＬとして出力する。

なお、投光器４により形成するテクスチャは、上記のパターンに限らず、ランダムドットなど、一様でない他のパターンの模様であってもよい。

そして、ロボット１は、カメラ３Ｒ，３Ｌによって取得したステレオ画像等を基に、周辺の移動環境の床形状や設置物等を認識しつつ移動する。

例えば、図１（ａ），（ｂ）に示すように、ロボット１の進行方向前方に段差部としての階段５０（図１（ａ）では昇り階段、図１（ｂ）では降り階段）が存在している状況において、ロボット１は、カメラ３Ｒ，３Ｌによって取得した進行方向前方の環境のステレオ画像を基に、カメラ３Ｒ，３Ｌの撮像領域に階段５０が存在することや、その階段５０の空間的な配置位置を認識する。

この場合、本実施形態におけるロボット１では、階段５０の空間的な配置位置を認識するために、階段５０の各段の踏面５１の先端側エッジ５２及び基端側エッジ５３のそれぞれの空間的な配置位置と、各踏面５１を包含する平面の空間的な姿勢（法線方向）とが推定される。

ここで、ロボット１が認識する階段５０の各踏面５１の先端側エッジ５２及び基端側エッジ５３について補足すると、図１（ａ）に示すロボット１の如く、階段５０をその下段側踏面５１が手前側、該下段側踏面５１の一段上側の上段側踏面５１が奥側となる視線で見た場合においては、各踏面５１の先端側エッジ５２は、該踏面５１の外形の境界線のうち、手前側で横方向（階段５０の幅方向）に直線状に延在する境界線である。また、この場合において、各踏面５１の基端側エッジ５３は、該踏面５１の外形の境界線のうち、該踏面５１の奥側で横方向に直線状に延在する境界線である。

また、図１（ｂ）に示すロボット１の如く、階段５０をその上段側踏面５１が手前側、該上段側踏面５１の一段下側の下段側踏面５１が奥側となる視線で見た場合にいては、各踏面５１の先端側エッジ５２は、上該踏面５１の外形の境界線のうち、奥側で横方向（階段５０の幅方向）に直線状に延在する境界線である。また、この場合において、各踏面５１の基端側エッジ５３は、該踏面５１の外形の境界線のうち、該踏面５１の手前側で横方向に直線状に延在する境界線である。

また、段差部としての階段の構造に関して図３を参照して補足すると、階段の各段の踏面の先端部は、一般に段鼻と言われる部分である。従って、階段の踏面の先端側エッジというのは、換言すれば、階段の各段の段鼻の上面の先端側境界線を意味する。また、階段の上段側踏面の先端側と、その一段下側の下段側踏面の基端側との間での部分は、一般に蹴込みと言われる。そして、階段の各段の踏面の先端部（段鼻）は、多くの場合、図３に示す如く、その先端面が、上記蹴込みの箇所に起立した姿勢で形成される面部（以降、蹴込み面という）よりも若干突き出るように形成される。

但し、本実施形態で、段差部の例として主に説明する階段では、段鼻の先端面が、蹴込み面よりも若干突き出ている必要はない。例えば、図１（ａ），（ｂ）に簡略的に記載した階段５０のように、段鼻の先端面と蹴込み面とが面一もしくはほぼ面一に形成されていてもよい。

また、本実施形態で、段差部の例として主に説明する階段は、蹴込みの箇所に、蹴込み面を形成する板部材が設けられていない（蹴込み面が存在しない）構造のものであってもよい。

図４を参照して、ロボット１には、ＣＰＵ、ＲＡＭ、ＲＯＭを含む電子回路ユニットにより構成された演算処理装置１０が搭載されている。そして、この演算処理装置１０にカメラ３Ｒ，３Ｌ及び投光器４が接続されている。

この演算処理装置１０は、カメラ３Ｒ，３Ｌ及び投光器４の動作制御を行なう機能を有しており、カメラ３Ｒ，３Ｌの撮像動作と、投光器４の補助光ＳＬの出力動作とを互いに同期した所要のタイミング（所定の制御処理周期のタイミング）で行なわせるように制御する。そして、演算処理装置１０は、カメラ３Ｒ，３Ｌによりそれぞれ取得される撮像画像の画素値データ（撮像画像を構成する画素毎の輝度、彩度、色相等を表すデータ）を取り込み、この画素値データを、図示しない画像メモリに逐次更新しつつ、記憶保持する。

なお、カメラ３Ｒ，３Ｌに入射する光の波長を制限するバンドパスフィルタを備えておき、補助光ＳＬの波長に一致もしくはほぼ一致する波長の光だけをカメラ３Ｒ，３Ｌで撮像するようにしてもよい。

さらに、演算処理装置１０は、実装されたプログラムによって実現される機能として、カメラ３Ｒ，３Ｌの撮像領域に段差部が存在しているか否かを検知する段差部存在検知部１１と、該撮像領域に存在する実際の段差部の平面部分をステレオ画像のうちの基準画像に投影してなる平面投影領域を抽出すると共に各平面投影領域に対応する平面部分の平面パラメータを決定（推定）する平面投影領域抽出部１２と、撮像領域に存在する実際の段差部の踏面の先端側エッジを基準画像に投影してなる実エッジ投影線を推定（特定）するための処理を実行するエッジ投影線推定処理部１３と、段差部の空間的な配置位置を特定する段差部空間配置決定部１４とを備えている。そして、演算処理装置１０は、これらの機能部１１〜１４の処理によって、本発明の段差部認識装置の一実施形態としての機能を実現する。

この場合、本実施形態では、カメラ３Ｒの撮像画像とカメラ３Ｌの撮像画像とのうちの一方、例えばカメラ３Ｒの撮像画像を基準画像、他方のカメラ３Ｌの撮像画像を参照画像として使用する。なお、演算処理装置１０は、ロボット１の動作制御を行なう機能を備えていてもよい。

以下に、上記段差部存在検知部１１、平面投影領域抽出部１２、エッジ投影線推定処理部１３、段差部空間配置決定部１４の処理を詳細に説明する。

カメラ３Ｒ，３Ｌによって取得されたロボット１の進行方向前方の撮像画像（ステレオ画像）の画素値データが段差部存在検知部１１に与えられる。そして、段差部存在検知部１１は、与えられたステレオ画像を使用して、図５のフローチャートに示す処理を実行する。

段差部存在検知部１１は、まず、ＳＴＥＰ１の処理を実行する。このＳＴＥＰ１では、段差部存在検知部１１は、与えられたステレオ画像のうちの基準画像内に、図６に例示する如く、段差部の存在の有無を検知するための領域である段差部存在検知用領域Ｒ１を設定する。

図６に示す基準画像は、カメラ３Ｒ，３Ｌの撮像領域に階段５０（図示例では昇り階段）が存在する場合の基準画像を概略的に例示したものである。この場合、階段５０の表面には前記投光器４から出力された補助光ＳＬによって、テクスチャとしての輝線ＳＬａ，ＳＬｂが付与される。この輝線ＳＬａ，ＳＬｂも基準画像に投影される。なお、図６では基準画像に投影される輝線ＳＬａ，ＳＬｂの一部を代表的に示している。

ＳＴＥＰ１で段差部存在検知部１１が設定する段差部存在検知用領域Ｒ１は、本実施形態では、その大きさ（基準画像の横方向及び縦方向での幅）があらかじめ定められた方形状の領域である。この段差部存在検知用領域Ｒ１の横方向の幅は、階段等の段差部が基準画像に投影されている場合に、該基準画像における段差部の横方向の幅内に十分に収まる程度の大きさに設定されている。

また、段差部存在検知用領域Ｒ１の縦方向の幅は、階段等の段差部が基準画像に投影されている場合に、段差部存在検知用領域Ｒ１内に複数段の踏面の画像が含まれるような大きさに設定されている。

そして、段差部存在検知用領域Ｒ１は、基準画像内で、ロボット１の進行予定領域（ロボット１を目標とする進行方向に移動させた場合に該ロボット１が通過する領域）に存在するような位置に配置される。

次いで、段差部存在検知部１１は、ＳＴＥＰ２の処理を実行する。このＳＴＥＰ２では、段差部存在検知部１１は、基準画像内に設定した段差部存在検知用領域Ｒ１の画像が、ある平面上の部分の投影画像であると仮定し、この仮定にできるだけ適合するような当該平面の空間的な位置及び姿勢を表す平面パラメータを、射影変換（詳しくは平面射影変換）を利用した手法によって決定する。

本実施形態では、上記平面パラメータとして、平面の法線方向の単位ベクトル↑ｎを、基準画像を撮像するカメラ３Ｒ（以降、基準カメラ３ということがある）の光学中心Ｃから該平面までの距離ｄにより除算してなるベクトル↑ｍ（≡↑ｎ／ｄ）を使用する。

例えば、図７（ａ）又は図７（ｂ）に二点鎖線で示す平面（図では仮想平面）の位置及び姿勢を表す平面パラメータは、図中に参照符号↑ｍ1を付した矢印で示すように、該平面に垂直で、且つ大きさが１／ｄのベクトルとして表現される。

以降、このように定義されるベクトル↑ｍを平面パラメータベクトルという。なお、本明細書では、“↑”は、ベクトル（縦ベクトル）を表現する記号として用いる。

ここで、上記射影変換は、一般的に言えば、ステレオ画像を構成する２つの撮像画像に、階段の踏面等の平面部分を投影した場合に、一方の撮像画像中における平面部分の画像を、他方の撮像画像における当該平面部分の画像に変換するものである。その変換を行なう射影変換行列Ｐは、カメラ３Ｒ，３Ｌの２つの撮像画像のそれぞれの画像座標（各撮像画像における各画素の位置を表す二次元座標）を同次座標で表現した場合に、公知のように次式（１−１）により表される行列である。

Ｐ＝Ｒ＋↑ｔ・↑ｍ^T ……（１−１）

式（１−１）におけるＲは、カメラ３Ｒ，３Ｌのそれぞれのカメラ座標系の間の回転行列、↑ｔはそれらのカメラ座標系の間の並進移動ベクトル、↑ｍ^Tは上記平面パラメータベクトル↑ｍを転置したベクトルである。

この場合、カメラ３Ｒのカメラ座標系は、図７（ａ），（ｂ）に示す如く、カメラ３Ｒに対して固定された座標系である。このカメラ座標系は、例えば、カメラ３Ｒの光学中心Ｃを原点として、光軸Ｌｃの方向をｚ軸方向、カメラ３Ｒの撮像面の上下方向（縦方向）をｙ軸方向、該撮像面の横方向をｘ軸方向とする３軸座標系である。左カメラ３Ｌのカメラ座標系についても同様である。

上記回転行列Ｒ及び並進移動ベクトル↑ｔは、カメラ３Ｒ，３Ｌの相互の相対的な位置関係及び姿勢関係に依存して定まるものであり、本実施形態ではあらかじめ特定されている。従って、射影変換行列Ｐは、平面パラメータベクトル↑ｍによって規定されるものである。

補足すると、基準画像から参照画像への射影変換行列と、これの逆変換の射影変換行列（参照画像から基準画像への射影変換行列）とは、いずれも、式（１−１）の右辺の形で表現される。ただし、上記Ｒ，↑ｔ，↑ｍの値は、それぞれの射影変換行列で異なる。

具体的には、式（１−１）により表現される射影変換行列Ｐを基準画像及び参照画像のうちの一方から他方へのへの射影変換行列とした場合、その逆変換の射影変換行列（以降、これをＰ^-1と表記する）は、次式（１−２）の但し書きの式（１−２ａ）〜（１−２ｄ）により定義されるＲ’、↑ｔ’、↑ｍ’、↑ｄ’を用いて、上記式（１−１）と同様の形の式（１−２）により表される。なお、Ｒ^T _、(↑ｍ’)^Tはそれぞれ、Ｒの転値行列、(↑ｍ’)の転値ベクトルである。

Ｐ^-1＝Ｒ'＋↑ｔ’・(↑ｍ’)^T ……（１−２）
但し、
Ｒ’≡Ｒ^T ……（１−２ａ）
↑ｔ’≡−Ｒ^T・↑ｔ ……（１−２ｂ）
↑ｍ’≡Ｒ・↑ｍ／ｄ’ ……（１−２ｃ）
↑ｄ’≡ｄ・（１＋↑ｍ^T・Ｒ^T・↑ｔ） ……（１−２ｄ）

以降の本実施形態の説明では、便宜上、基準画像から参照画像への射影変換行列を式（１−１）により表される射影変換行列Ｐとし、参照画像から基準画像への射影変換行列を式（１−２）により表される射影変換行列Ｐ^-1とする。また、平面パラメータベクトル↑ｍは、便宜上、基準カメラ３（本実施形態ではカメラ３Ｒ）のカメラ座標系で見たベクトルとして表記されるものとする。

基準画像における段差部存在検知用領域Ｒ１の画像が、ある平面上の部分（平面部分）の投影画像である場合には、その段差部存在検知用領域Ｒ１の画像と、該段差部存在検知用領域Ｒ１に対応する参照画像中の領域（詳しくは、基準画像の段差部存在検知用領域Ｒ１に投影されている実空間（実際の空間）の平面部分を、参照画像に投影してなる領域）の画像との間の関係は、上記射影変換によって対応づけられる。

より詳しくは、基準画像の段差部存在検知用領域Ｒ１に対応する参照画像中の領域の画像を、当該平面部分の平面パラメータベクトル↑ｍに応じて上記式（１−２）に規定される射影変換行列Ｐ^-1により射影変換した場合に得られる射影変換画像の画素値分布は、基準画像の段差部存在検知用領域Ｒ１の画像の画素値分布に一致もしくはほぼ一致することとなる。

なお、上記射影変換画像の各画素位置（基準画像の画像座標系で見た位置）での画素値は、その画素位置を、当該平面部分の平面パラメータベクトル↑ｍに応じて上記式（１−１）により規定される射影変換行列Ｐによって変換してなる参照画像内の画素位置（参照画像の画像座標系で見た位置）の画素値に一致する。従って、上記射影変換画像は、換言すれば、基準画像の段差部存在検知用領域Ｒ１の全ての画素位置での画素値を、射影変換行列Ｐにより該段差部存在検知用領域Ｒ１の各画素位置に対応付けられる参照画像内の画素位置の画素値で置き換えてなる画像に一致する。

ＳＴＥＰ２の処理では、段差部存在検知部１１は、基準画像内に設定した段差部存在検知用領域Ｒ１に平面部分が投影されていると仮定した場合の当該仮想的な平面（仮想平面）の平面パラメータベクトル↑ｍを、以下に説明する処理によって決定する。

すなわち、段差部存在検知部１１は、平面パラメータベクトル↑ｍの暫定値（候補値）を設定する処理、基準画像の段差部存在検知用領域Ｒ１の画像の画素値分布と、設定した↑ｍの暫定値により規定される射影変換によって、段差部存在検知用領域Ｒ１に対応する参照画像中の領域の画像を射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す誤差関数Ｄ１の値を算出する処理、並びに、この誤差関数Ｄ１の値に応じて平面パラメータベクトル↑ｍの暫定値を更新する処理を、既定の所定条件が満たされるまで繰り返す。

そして、段差部存在検知部１１は、上記所定条件が満たされたときの↑ｍの暫定値を、基準画像の段差部存在検知用領域Ｒ１に投影されている仮想平面の平面パラメータベクトル↑ｍとして決定する。以降、このように段差部存在検知部１１が決定する平面パラメータベクトル↑ｍを段差部存在検知用平面パラメータベクトル↑ｍ１という。

ここで、基準画像の段差部存在検知用領域Ｒ１内の任意の画素位置Ｑでの画素値をＩ１(Ｑ)、基準画像上の画素位置Ｑを平面パラメータベクトル↑ｍの暫定値により規定される前記式（１−１）の射影変換行列Ｐにより射影変換してなる参照画像上の画素位置をｆ(Ｑ)、この画素位置ｆ(Ｑ)での参照画像の画素値をＩ２(ｆ(Ｑ))、段差部存在検知用領域Ｒ１内のトータルの画素数をＮ１とする。このとき、上記誤差関数Ｄ１は、例えば次式（２−１）により与えられる関数である。

なお、撮像画像（基準画像又は参照画像）の各画素の画素値は、本実施形態では、該画素における撮像画像の輝度値である。ただし、撮像画像がカラー画像である場合には、撮像画像の色相又は彩度を画素値として使用してもよい。

Ｄ１＝（１／Ｎ１）・Σ(Ｉ１(Ｑ)−Ｉ２(ｆ(Ｑ)))² ……（２−１）

式（２−１）のΣは、段差部存在検知用領域Ｒ１内の全ての画素についての総和を意味する。従って、式（２−１）により定義される誤差関数Ｄ１は、基準画像の段差部存在検知用領域Ｒ１での、画素値Ｉ１(Ｑ)と、これに対応する参照画像の射影変換画像の画素値Ｉ２(ｆ(Ｑ))との差の二乗値の平均値を、段差部存在検知用領域Ｒ１における基準画像の画素値分布と、これに対応する参照画像の射影変換画像の画素値分布との誤差として表現する関数である。

なお、式（２−１）により与えられる誤差関数Ｄの代わりに、例えば次式（２−２）により与えられる誤差関数Ｄ１を使用してもよい。

Ｄ１＝（１／Ｎ１）・Σ｜(Ｉ１(Ｑ)−Ｉ２(ｆ(Ｑ))｜ ……（２−２）

この式（２−２）により表される誤差関数Ｄ１は、基準画像の段差部存在検知用領域Ｒ１での、Ｉ１(Ｑ)とＩ２(ｆ(Ｑ))との差の絶対値の平均値を、段差部存在検知用領域Ｒ１における基準画像の画素値分布と、これに対応する参照画像の射影変換画像の画素値分布との誤差として表現する関数である。

上記の如く算出される誤差関数Ｄ１の値は、↑ｍの暫定値に依存して変化する。そして、段差部存在検知用領域Ｒ１に投影されている部分が、実際の平面部分である場合には、↑ｍの暫定値が該平面部分を包含する平面の平面パラメータベクトルに一致する場合に、誤差関数Ｄ１の値は最小（ほぼゼロ）になる。

従って、誤差関数Ｄ１の値が最小（もしくはほぼ最小）となるような↑ｍの暫定値が、段差部存在検知用領域Ｒ１に投影されている仮想的な平面部分を包含する仮想平面の位置及び姿勢を表す平面パラメータベクトルとしての適合度が高いものとなる。

そこで、段差部存在検知部１１は、この誤差関数Ｄ１に応じて↑ｍの暫定値を更新する処理では、公知の収束演算処理手法によって、この誤差関数Ｄ１の値を、その最小値により近づけ得るように↑ｍの暫定値を更新する。そして、段差部存在検知部１１は、このような↑ｍの暫定値の更新を所定条件が成立するまで実行することで、最終的に、上記誤差関数Ｄ１の値を最小化（もしくはほぼ最小化）し得るような前記段差部検知用平面パラメータベクトル↑ｍ１を決定する。

この場合、上記所定条件は、その条件が成立する場合に、更新後の↑ｍが、誤差関数Ｄを最小化し得る値に一致もしくはほぼ一致するとみなし得るものとして、あらかじめ定められた条件である。該所定条件は、例えば、更新前の↑ｍに応じて算出される誤差関数Ｄ１の値と、更新後の↑ｍに応じて算出される誤差関数Ｄ１の値との差の絶対値が、あらかじめ定められた所定の閾値（“０”に近い閾値）以下となるという条件、あるいは、↑ｍの暫定値の更新回数が所定回数を超えるという条件等の条件である。

なお、↑ｍの暫定値の初期値としては、例えば、ロボット１が現在接地している面（床面等）の平面パラメータベクトルを使用することができる。ただし、該初期値は、あらかじめ定めた固定値であってもよい。また、該初期値は、ロボット１の現在の接地面に対して傾いた平面の平面パラメータベクトルであってもよい。

次いで、段差部存在検知部１１は、ＳＴＥＰ３の処理を実行する。このＳＴＥＰ３では、段差部存在検知部１１は、ＳＴＥＰ２で決定した段差部存在検知用平面パラメータベクトル↑ｍ１の向きに基づいて、カメラ３Ｒ，３Ｌの撮像領域内に段差部が存在しているか否かを判断する。

ここで、カメラ３Ｒ，３Ｌの撮像領域に段差部が存在せず、該撮像領域がほぼ平坦な床面である場合には、ＳＴＥＰ２において上記の如く決定された段差部存在検知用平面パラメータベクトル↑ｍ１の向きは、ロボット１の現在の接地面の法線方向とほぼ同方向になる。

一方、カメラ３Ｒ，３Ｌの撮像領域に階段等の段差部が存在している場合には、ＳＴＥＰ２で決定した段差部存在検知用平面パラメータベクトル↑ｍ１の向きは、ロボット１の現在の接地面の法線方向に対して比較的大きく傾いたものとなる。

例えば図７（ａ）に示すように、カメラ３Ｒ，３Ｌの撮像領域に昇り階段５０が存在する場合、あるいは、図７（ｂ）に示すように、カメラ３Ｒ，３Ｌの撮像領域に降り階段５０が存在する場合には、段差部存在検知部１１が決定する段差部存在検知用平面パラメータベクトル↑ｍ１の向きは、ピッチ方向（ロボット１の進行方向を横切る方向（ロボット１の左右方向）の軸周り方向）において、ロボット１の現在の接地面の法線方向に対して比較的大きく傾くものとなる。

そこで、段差部存在検知部１１は、ＳＴＥＰ３では、決定した段差部存在検知用平面パラメータベクトル↑ｍ１が、ロボット１の現在の接地面の法線方向に対してピッチ方向に所定角度以上、傾いているか否かによって、カメラ３Ｒ，３Ｌの撮像領域に階段等の段差部が存在しているか否かを判断する。

具体的には、段差部存在検知部１１は、ロボット１の現在の接地面の法線方向に対する↑ｍ１の傾き角度（ピッチ方向での傾き角度）の大きさが、既定の所定角度以上である場合には、カメラ３Ｒ，３Ｌの撮像領域に階段等の段差部が存在していると判断する。また、段差部存在検知部１１は、ロボット１の現在の接地面の法線方向に対する↑ｍ１の傾き角度（ピッチ方向での傾き角度）が所定角度未満である場合には、カメラ３Ｒ，３Ｌの撮像領域に階段等の段差部が存在していないと判断する。

なお、ロボット１の現在の接地面と、カメラ３Ｒ，３Ｌとの間の相対的な位置及び姿勢関係は、ロボット１の各関節の変位量（詳しくは、ロボット１の接地している脚とカメラ３Ｒ，３Ｌとの間に存在する各関節の変位量）に応じて定まる。従って、ロボット１の現在の接地面の法線方向に対する↑ｍ１の傾き角度は、基準カメラ３（カメラ３Ｒ）のカメラ座標系で見た↑ｍ１の値と、ロボット１の各関節の変位量の計測値（又は該変位量を追従させる目標値）とから算出することができる。

そして、段差部存在検知部１１は、ＳＴＥＰ３の判断結果が肯定的である場合（段差部が存在していると判断した場合）には、さらにＳＴＥＰ４の処理を実行する。このＳＴＥＰ４では、カメラ３Ｒ，３Ｌの撮像領域に存在する段差部の種別（昇りの段差部であるか降りの段差部であるかの種別）を判断する。

ここで、カメラ３Ｒ，３Ｌの撮像領域に、図７（ａ）に示す如く、昇りの段差部（図では昇り階段５０）が存在する場合には、段差部存在検知部１１が決定する段差部存在検知用平面パラメータベクトル↑ｍ１は、ロボット１から見て、昇り勾配斜面の平面パラメータベクトルとなる。また、カメラ３Ｒ，３Ｌの撮像領域に、図７（ｂ）に示す如く、降りの段差部（図では降り階段５０）が存在する場合には、段差部存在検知部１１が決定する段差部存在検知用平面パラメータベクトル↑ｍ１は、ロボット１から見て、降り勾配斜面の平面パラメータベクトルとなる。

そこで、ＳＴＥＰ４では、段差部存在検知部１１は、↑ｍ１が、ロボット１の現在の接地面の法線方向に対して、ピッチ方向で正負のいずれの向きに傾いているかによって、該段差部が昇りの段差部であるか、降りの段差部であるかを判断する。

以上が、段差部存在検知部１１の処理の詳細である。

段差部存在検知部１１により、カメラ３Ｒ，３Ｌの撮像領域に段差部が存在することが検知された場合には、演算処理装置１０は、次に、平面投影領域抽出部１２の処理を実行する。

この平面投影領域抽出部１２は、カメラ３Ｒ，３Ｌによって取得された撮像画像（ステレオ画像）を用いて、図８のフローチャートに示す処理を実行する。

平面投影領域抽出部１２は、まず、ＳＴＥＰ１１の処理を実行する。このＳＴＥＰ１１では、平面投影領域抽出部１２は、与えられたステレオ画像のうちの基準画像内の複数の（Ｎ２個の）位置(ｕ1，ｖ(i))（ｉ＝１，２，…，Ｎ2）に、微小な面積の領域である局所領域Ｒ２(i)を設定する。

各局所領域Ｒ２(i)は、図９に示す如く例えば方形状の領域である。そして、各局所領域Ｒ２(i)のサイズ（縦方向及び横方向の幅）は、基準画像に段差部の画像が投影されている場合に、該段差部の各段の踏面内に収まり得るようなサイズにあらかじめ設定されている。

なお、図９では、前記投光器４から出力される補助光ＳＬによる斜行輝線ＳＬａ，ＳＬｂの図示は省略している。このことは、後述する図１３、図１９においても同様である。

補足すると、各局所領域Ｒ２(i)の形状は、方形状でなくともよく、例えば、平行四辺形や台形等の形状であってもよい。

各局所領域Ｒ２(i)の配置位置(ｕ1，ｖ(i))は、基準画像の横方向をｕ軸、縦方向（上下方向）をｖ軸とする画像座標系で表した、各局所領域Ｒ２(i)の代表点の位置である。その代表点は、図９に示す例では、各局所領域Ｒ２(i)の左上隅の点である。ただし、該代表点は、各局所領域Ｒ２(i)の他の点、例えば各局所領域Ｒ２(i)の中央点、右上隅の点、左下隅の点、右下隅の点等であってもよい。

本実施形態では、各局所領域Ｒ２(i)の配置位置(ｕ1，ｖ(i))のうち、基準画像の横方向（ｕ軸方向）での位置ｕ1は互いに同一の位置である。その位置ｕ1は、基準画像におけるロボット１の通過予定領域上に各局所領域Ｒ２(i)が配置されるように設定される。例えば、位置ｕ1は、ロボット１の通過予定領域の幅方向中央付近の位置に設定される。

また、各局所領域Ｒ２(i)の配置位置(ｕ1，ｖ(i))のうち、基準画像の縦方向（ｖ軸方向）の位置ｖ(i)（ｉ＝１，２，…，Ｎ2）は、基準画像の下部から上部まで、所定の微小間隔（例えば１画素分の間隔）を存して並ぶようにあらかじめ設定された位置である。

そして、ＳＴＥＰ１１では、平面投影領域抽出部１２は、上記の如く配置位置が決定された各局所領域Ｒ２(i)のそれぞれに対して、基準画像の各局所領域Ｒ２(i)に投影されている部分が平面部分であると仮定した場合の当該平面部分の平面パラメータベクトル↑ｍを算出する。以降の説明では、各局所領域Ｒ２(i)（ｉ＝１，２，…，Ｎ2）のそれぞれに対応して算出される平面パラメータベクトル↑ｍを参照符号↑ｍ_r2(i)を用いて表記する。

この場合、各局所領域Ｒ２(i)（ｉ＝１，２，…，Ｎ2）のそれぞれに対応する平面パラメータベクトル↑ｍ_r2(i)の算出は、前記段差部存在検知部１１によるＳＴＥＰ２の処理と同様の処理によって行なわれる。この場合、↑ｍ_r2(i)の算出処理は、前記段差部存在検知用領域Ｒ１の代わりに、局所領域Ｒ２(i)を使用する（前記誤差関数Ｄ１の値を算出するための領域としてＲ２(i)を使用する）こと以外は、ＳＴＥＰ２の処理と同じでよい。

次いで、平面投影領域抽出部１２は、ＳＴＥＰ１２の処理を実行する。このＳＴＥＰ１２では、平面投影領域抽出部１２は、上記の如く算出した各局所領域Ｒ２(i)（ｉ＝１，２，…，Ｎ2）に対応する平面パラメータベクトル↑ｍ_r2(i)から、この↑ｍ_r2(i)により位置及び姿勢が規定される平面（Ｒ２(i)に投影されているとみなす仮想的な平面）の法線方向の単位ベクトルである単位法線ベクトル↑ｎ_r2(i)を算出する。

この単位法線ベクトル↑ｎ_r2(i)は、平面パラメータベクトル↑ｍ_r2(i)と同じ向きを有する単位長のベクトルであるから、↑ｍ_r2(i)を、その大きさ（＝｜↑ｍ_r2(i)｜）により除算することによって算出される。

次いで、平面投影領域抽出部１２は、ＳＴＥＰ１３の処理を実行する。このＳＴＥＰ１３では、平面投影領域抽出部１２は、各局所領域Ｒ２(i)における単位法線ベクトル↑ｎ_r2(i)と、現在のロボット座標系のＺ軸方向の単位ベクトル↑ｎzとの内積（スカラー積）を算出する。

上記ロボット座標系というのは、図７（ａ）又は図７（ｂ）に示す如く、ロボット１の現在の接地面に対して固定された座標系であって、該接地面の法線方向をＺ軸方向、ロボット１の進行方向をＸ軸方向、これらのＺ軸及びＸ軸に直交する方向（ロボット１の左右方向）をＹ軸方向とする座標系である。従って、現在のロボット座標系のＺ軸方向の単位ベクトル↑ｎzというのは、ロボット１の現在の接地面の法線方向の単位ベクトルを意味する。

そして、この単位ベクトル↑ｎzと、単位法線ベクトル↑ｎ_r2(i)との内積を算出する場合、基準カメラ３のカメラ座標系で見たベクトルとしての単位法線ベクトル↑ｎ_r2(i)が、ロボット１の各関節の変位量の計測値（又は目標値）に応じて、現在のロボット座標系で見たベクトルに座標変換される。そして、この座標変換後の単位法線ベクトル↑ｎ_r2(i)と、ロボット１の現在の接地面の法線方向（現在のロボット座標系のＺ軸方向）の単位ベクトル↑ｎzとの内積が算出される。

なお、カメラ座標系で見た単位法線ベクトル↑ｎ_r2(i)と、ロボット１の現在の接地面の法線方向の単位ベクトル↑ｎzをロボット座標系からカメラ座標系に座標変換したベクトルとの内積を算出するようにしてもよい。

これにより、図１０に例示する如く、基準画像の縦方向での各局所領域Ｒ２(i)の位置ｖ(i)と、上記内積の値との間の関係を表すデータが得られることとなる。

ここで、カメラ３Ｒ，３Ｌの撮像領域に段差部が存在する場合、その段差部の各踏面は、一般に、ロボット１の現在の接地面とほぼ平行な平面部分となる。従って、基準画像における段差部の踏面の投影領域内に含まれる各局所領域Ｒ２(i)（例えば、図９に示すＲ２(1)，Ｒ２(ib)，Ｒ２(N2)）に対応して上記の如く算出される内積の値は、“１”またはそれに近い値となる。

一方、段差部の踏面の投影領域に含まれない各局所領域Ｒ２(i)、又は、段差部の踏面の投影領域が一部しか含まれないような各局所領域Ｒ２(i)（例えば、図９に示すＲ２(ia)）に対応する内積の値は、“１”から乖離した値となる。

従って、上記内積の値は、各局所領域Ｒ２(i)に投影されている部分が、段差部の踏面の部分であるか否かを示す指標となるものである。

次いで、平面投影領域抽出部１２は、ＳＴＥＰ１４の処理を実行する。このＳＴＥＰ１４では、平面投影領域抽出部１２は、Ｎ2個の局所領域Ｒ２(i)（ｉ＝１，２，…，Ｎ2）のうち、ＳＴＥＰ１３で算出した内積の値が、あらかじめ定めれた所定の閾値Th2以上となる各局所領域Ｒ２(i)について、該局所領域Ｒ２(i)に投影されている部分の、ロボット座標系で見たＺ軸座標位置（すなわち、ロボット１の現在の接地面に対する当該投影部分の高さ）を、該局所領域Ｒ２(i)に対応する平面パラメータベクトル↑ｍ_r2(i)を基に算出する。

ここで、上記所定の閾値Th2は、“１”よりも若干小さい値、例えば０．９９９である。従って、内積の値が、この閾値Th2以上となる各局所領域Ｒ２(i)は、それに投影されている部分が、ロボット１の現在の接地面とほぼ平行になっているような部分である。

そして、閾値Th2以上となる各局所領域Ｒ２(i)に対応する平面パラメータベクトル↑ｍ_r2(i)により規定される平面と、ロボット１の現在の接地面を含む平面との間の間隔（詳しくは、ロボット座標系のＺ軸方向での間隔）を算出することによって、該局所領域Ｒ２(i)に投影されている部分のＺ軸座標位置（ロボット１の現在の接地面に対する高さ）が決定される。

次いで、平面投影領域抽出部１２は、ＳＴＥＰ１５の処理を実行する。このＳＴＥＰ１５では、平面投影領域抽出部１２は、前記ＳＴＥＰ１４で得られたデータを基に、基準画像における段差部の踏面に対応する平面投影領域を抽出（特定）する。

具体的には、平面投影領域抽出部１２は、内積の値が前記閾値Th2以上となるｖ軸方向の区間であって、その区間内の各位置の局所領域Ｒ２(i)に投影されている部分の高さ（ロボット１の接地面からの高さ）がほぼ一定となる（各投影部分の相互の高さの差の大きさがあらかじめ定めた所定の閾値以下となる）区間を決定する。なお、各投影部分の高さの相互の差の大きさ（絶対値）に対する閾値は、“０”に近い微小値、例えば１０［ｍｍ］である。

図１０に示すΔｖ１、Δｖ２、Δｖ３は、このようにして決定される区間の例を示すものである。このようにして決定される各区間は、ＳＴＥＰ１１で設定したＮ2個の局所領域Ｒ２(i)（ｉ＝１，２，…，Ｎ2）のうち、ロボット１の現在の接地面とほぼ平行な単一の平面部分（段差部の各踏面に相当する平面部分）の投影領域となっている局所領域Ｒ２(i)の代表点の位置（ｖ軸方向の位置）の範囲に相当するものである。以降、このようにして決定される各区間を同一平面投影区間という。

そして、平面投影領域抽出部１２は、上記の如く決定した各同一平面投影区間毎に、それに対応する平面投影領域を決定する。この場合、本実施形態では、各平面投影領域は、基準画像の横方向（ｕ軸方向）に局所領域Ｒ２(i)（ｉ＝１，２，…，Ｎ2）と同じ幅を有し、且つ、その横方向の範囲（各平面投影領域の左端と右端との間の範囲）が局所領域Ｒ２(i)の横方向の範囲に一致するようにして、基準画像に配置される方形状の領域である。

そして、基準画像の縦方向（ｖ軸方向）での各平面投影領域の範囲（各平面投影領域の上端と下端との間の範囲）は、該平面投影領域に対応する同一平面投影区間内に代表点の位置を有する全ての局所領域Ｒ２(i)を合成した領域の縦方向の範囲と一致するように決定される。

例えば、基準画像のｖ軸方向の位置がｖ(ic)からｖ(ic+n)までの区間が、ある１つの同一平面投影区間であるとしたとき、その同一平面投影区間に対応する平面投影領域の縦方向（ｖ軸方向）の範囲は、該同一平面投影区間内に代表点の位置を有するｎ＋１個の局所領域Ｒ２(ic)〜Ｒ２(ic+n)のうち、最も下側の局所領域Ｒ２(ic)の下端位置と、最も上側の局所領域Ｒ２(ic+n)の上端位置との間の範囲として決定される。

以上のＳＴＥＰ１５の処理によって、段差部の各踏面に対応する平面投影領域が、基準画像から抽出されることとなる。例えば、図９に破線枠で示す方形状の領域ＡＲ２が、平面投影領域として基準画像から抽出される。

次いで、平面投影領域抽出部１２は、ＳＴＥＰ１６の処理を実行する。このＳＴＥＰ１６では、平面投影領域抽出部１２は、ＳＴＥＰ１５の処理によって２つ以上の平面投影領域ＡＲ２が抽出されたか否かを判断する。そして、この判断結果が否定的である場合には、基準画像に段差部が写っていない可能性があるので、図８の処理（今回の演算処理周期での処理）を終了する。

一方、ＳＴＥＰ１６の判断結果が肯定的である場合には、平面投影領域抽出部１２は、ＳＴＥＰ１７の処理を実行した後、図８の処理を終了する。このＳＴＥＰ１７では、平面投影領域抽出部１２は、各平面投影領域ＡＲ２に投影されている平面部分を包含する平面の平面パラメータベクトル↑ｍ２を決定する。

この場合、平面投影領域抽出部１２は、例えば、前記局所領域Ｒ２(i)（ｉ＝１，２，…，Ｎ2）のうち、各平面投影領域ＡＲ２に含まれる局所領域Ｒ２(i)のそれぞれに対応する平面パラメータベクトル↑ｍ_r2(i)のそれぞれの方位角（基準カメラ３のカメラ座標系で見た方位角）の平均の方位角を有する単位ベクトルを、該平面投影領域ＡＲ２に対応する平面の単位法線ベクトル↑ｎ２として決定する。

さらに、平面投影領域抽出部１２は、各平面投影領域ＡＲ２に含まれる局所領域Ｒ２(i)のそれぞれに対応して、前記ＳＴＥＰ１１で推定した平面パラメータベクトル↑ｍ_r2(i)の大きさ（絶対値）の逆数値（＝１／｜↑ｍ(i)｜）の平均値を、該平面投影領域ＡＲ２に対応する平面の、基準カメラ３の光学中心Ｃからの距離ｄ２として決定する。

そして、各平面投影領域ＡＲ２に対応する平面の単位法線ベクトル↑ｎ２を距離ｄ２で除算してなるベクトル（＝↑ｎ２／ｄ２）を該平面投影領域ＡＲ２に対応する平面パラメータベクトル↑ｍ２として決定する。

なお、各局所領域Ｒ２(i)に対応する平面パラメータベクトル↑ｍ_r2(i)を推定する場合と同様の処理（射影変換を利用した収束演算処理）によって、各平面投影領域ＡＲ２に対応する平面パラメータベクトル↑ｍ２を改めて推定するようにしてもよい。

以上が平面投影領域抽出部１２の処理の詳細である。

補足すると、本実施形態では、ＳＴＥＰ１５の処理で前記同一平面投影区間を決定する場合に、各局所領域Ｒ２(i)（ｉ＝１，２，…，Ｎ2）毎の前記内積の値に加えて、各局所領域Ｒ２(i)に投影されている部分の高さを参照するようにしたが、内積の値だけから、同一平面投影区間を決定するようにしてもよい。

例えば、内積の値が、基準画像の縦方向（ｖ軸方向）で連続的に所定の閾値Th2以上となる区間（その区間内に代表点の位置を有して、縦方向に並ぶ全ての局所領域Ｒ２(i)のそれぞれに対応する内積の値が閾値Th2以上となる区間）を同一平面投影区間として決定してもよい。このようにした場合には、ＳＴＥＰ１４の処理は不要である。

また、カメラ３Ｒ，３Ｌの撮像領域に存在する段差部の踏面に対応する平面投影領域だけでなく、段鼻の先端面又は蹴り込み面のように、踏面に対して起立する平面部分に対応する平面投影領域を抽出するようにしてもよい。

この場合、例えば各局所領域Ｒ２(i)（ｉ＝１，２，…，Ｎ2）毎の平面パラメータベクトル↑ｍ_r2(i)（又は単位法線ベクトル↑ｎ_r2(i)）の向きに着目し、その向きが連続的にほぼ一定に保たれる（ロボット１の現在の接地面の法線方向と同方向の向きでなくてもよい）ような区間を同一平面投影区間として決定する。そして、その決定した各同一平面投影区間に応じて上記と同様に平面投影領域を決定する。これにより、段差部の踏面に対応する平面投影領域だけでなく、段鼻の先端面や段差部の蹴り込み面の如く、踏面に対して起立する平面部分に対応する平面投影領域を抽出することもできる。

以上説明した平面投影領域抽出部１２によって、段差部の踏面に対応する２つ以上の平面投影領域ＡＲ２が抽出された場合には、演算処理装置１０は、次に、エッジ投影線推定処理部１３の処理を実行する。

このエッジ投影線推定処理部１３は、カメラ３Ｒ，３Ｌによって取得された撮像画像（ステレオ画像）と、平面投影領域抽出部１２で決定された各平面投影領域ＡＲ２に対応する平面パラメータベクトル↑ｍ２とを用いて、図１１及び図１２のフローチャートに示す処理を実行することで、上段側踏面の先端側エッジを基準画像に投影してなる実エッジ投影線を推定したものとしての推定エッジ投影線を特定する。

なお、基準画像における実エッジ投影線を推定するということは、基準画像における実エッジ投影線の位置及び方向（又はこれらを規定するパラメータ）を決定することを意味する。

エッジ投影線推定処理部１３は、まず、ＳＴＥＰ２１の処理を実行する。このＳＴＥＰ２１では、エッジ投影線推定処理部１３は、図５のＳＴＥＰ４の処理によって前記段差部存在検知部１１が特定した段差部（カメラ３Ｒ，３Ｌの撮像領域に存在する段差部）の種別が、ロボット１から見て、昇りの段差部であるか降りの段差部であるかを確認する。

エッジ投影線推定処理部１３は、カメラ３Ｒ，３Ｌの撮像領域に存在する段差部が、昇りの段差部である場合には、図１１に示すＳＴＥＰ２２〜３６の処理を実行し、降りの段差部である場合には、図１２に示すＳＴＥＰ３７〜５１の処理を実行する。

段差部が昇りの段差部である場合には、エッジ投影線推定処理部１３は、まず、ＳＴＥＰ２２において、基準画像内に、実エッジ投影線の推定用の画像処理領域であるエッジ推定用領域Ｒ３uを設定する。

ここで、以降の説明では、説明の便宜上、カメラ３Ｒ，３Ｌの撮像領域に存在する昇りの段差部が、一例として、図１（ａ）に示した如き昇り階段５０であるとする。そして、この昇り階段５０が、図１３に示す如く基準画像に写っているものとする。

さらに、この昇り階段５０の任意の１つの下段側踏面５１（例えばロボット１から見て最も手前側の下段側踏面）の一段上側の上段側踏面５１の先端側エッジ５２を基準画像に投影してなる実エッジ投影線Ｌ52rを推定する処理をエッジ投影線推定処理部１３により実行するものとする。

そして、推定しようとする実エッジ投影線Ｌ52rに対応する先端側エッジ５２を先端に有する上段側踏面５１と、その一段下側の下段側踏面５１とを、それぞれ参照符号５１ｐ，５１ｑを使用して表記する。

また、前記平面投影領域抽出部１２によって、上段側踏面５１ｐに対応して抽出された平面投影領域ＡＲ２を、以降、参照符号ＡＲ２ｐを使用して上段側踏面投影領域ＡＲ２ｐと称する。そして、この上段側踏面投影領域ＡＲ２ｐに対応して決定された平面パラメータベクトル↑ｍ２を参照符号↑ｍ2pを使用して表記する。

また、前記平面投影領域抽出部１２によって、下段側踏面５１ｑに対応して抽出された平面投影領域ＡＲ２を、以降、参照符号ＡＲ２ｑを使用して下段側踏面投影領域ＡＲ２ｑと称する。そして、この下段側踏面投影領域ＡＲ２ｑに対応して決定された平面パラメータベクトル↑ｍ２を参照符号↑ｍ2qを使用して表記する。

ＳＴＥＰ２２において基準画像に設定するエッジ推定用領域Ｒ３uは、本実施形態では、図１３に示す如く設定される方形状の領域である。具体的には、エッジ推定用領域Ｒ３uは、その下端の位置が下段側踏面投影領域ＡＲ２ｑの縦方向の幅内の位置（例えば下段側踏面投影領域ＡＲ２ｑの上端から既定の所定量だけ下側の位置）となり、且つ、Ｒ３uの上端の位置が上段側踏面投影領域ＡＲ２ｐの縦方向の幅内の位置（例えば上段側踏面投影領域ＡＲ２ｐの下端から所定量だけ上側の位置）になるように配置される。この場合、エッジ推定用領域Ｒ３uの縦方向の幅は、下段側踏面投影領域ＡＲ２ｑと上段側踏面投影領域ＡＲ２ｐとの間の縦方向の間隔に応じて決定される。

また、エッジ推定用領域Ｒ３uは、その横方向の範囲（左端と右端との間の範囲）が、下段側踏面投影領域ＡＲ２ｑ及び上段側踏面投影領域ＡＲ２ｐの横方向の範囲に一致するように配置される。この場合、エッジ推定用領域Ｒ３uの横方向の幅は、下段側踏面投影領域ＡＲ２ｑ及び上段側踏面投影領域ＡＲ２ｐの横方向の幅と同じ幅（既定の幅）に決定される。

なお、エッジ推定用領域Ｒ３uの横方向の幅は、下段側踏面投影領域ＡＲ２ｑ及び上段側踏面投影領域ＡＲ２ｐの横方向の幅よりも若干小さくてもよい。その場合は、エッジ推定用領域Ｒ３uは、その横方向の範囲が、下段側踏面投影領域ＡＲ２ｑ及び上段側踏面投影領域ＡＲ２ｐの横方向の範囲内に収まるように配置すればよい。

以上のＳＴＥＰ２２の処理によって、エッジ推定用領域Ｒ３uは、その内部に（上端の辺と下端の辺との間）に推定しようとする単一の実エッジ投影線Ｌ52rが存在し、且つ、該エッジ推定用領域Ｒ３uの上端の辺及び下端の辺がそれぞれ、上段側踏面投影領域ＡＲ２ｐ、下段側踏面投影領域ＡＲ２ｑに含まれるようにして、基準画像内に設定されることとなる。

エッジ投影線推定処理部１３は、次にＳＴＥＰ２３の処理を実行する。このＳＴＥＰ２３では、エッジ投影線推定処理部１３は、実エッジ投影線Ｌ52rが基準画像の横方向（ｕ軸方向）に延在するものと仮定して、図１４に示す如く、基準画像の縦方向（ｖ軸方向）における実エッジ投影線Ｌ52rの位置の候補としての複数（Ｎ3a個）の候補位置ｖ(k1)（ｋ1＝１，２，…，Ｎ3a）を決定する。

この場合、Ｎ3a個の候補位置ｖ(k1)（ｋ1＝１，２，…，Ｎ3a）は、エッジ推定用領域Ｒ３uの縦方向の範囲（上端と下端との間の範囲）内で、あらかじめ定められた所定の刻み間隔ずつ（例えば１画素分の間隔ずつ）、基準画像の縦方向に異なる位置とされる。また、これらの候補位置ｖ(k1)のうち、最も上側の候補位置としての第１番目の候補位置ｖ(1)は、エッジ推定用領域Ｒ３uの上端から所定間隔（例えば１画素分の間隔）だけ下側の位置に決定される。

さらに、最も下側の候補位置としての第Ｎ3a番目の候補位置ｖ(Ｎ3a)は、本実施形態では、この候補位置ｖ(N3a)に応じて後述する射影変換領域Ｒ３u3の上端位置を設定したときに、該射影変換領域Ｒ３u3の上端位置が、エッジ推定用領域Ｒ３uの下端から、該エッジ推定用領域Ｒ３u3の縦方向の所定の最小幅（例えば１画素分の幅）だけ上側の位置になるように、該エッジ推定用領域Ｒ３u3の縦方向の最小幅での上端位置に応じて決定される。

次に、エッジ投影線推定処理部１３は、ＳＴＥＰ２４の処理を実行する。この処理では、各候補位置ｖ(k1)に対応する処理を順番に実行するために、k1の初期値として“１”が設定される。

次いで、エッジ投影線推定処理部１３は、ＳＴＥＰ２５の処理を実行する。このＳＴＥＰ２５では、エッジ投影線推定処理部１３は、基準画像の縦方向での実エッジ投影線Ｌ52rの位置が第ｋ1番目の候補位置ｖ(k1)（ｋ1は現在設定されている値）に一致するものと仮定して、エッジ推定用領域Ｒ３u内に、図１４に例示する如く、３つの射影変換対象領域Ｒ３u1、Ｒ３u2、Ｒ３u3を設定する。

射影変換対象領域Ｒ３u1〜Ｒ３u3のうち、射影変換対象領域Ｒ３u1，Ｒ３u2は、第ｋ1番目の候補位置ｖ(k1)で横方向に延在する仮想的なエッジ投影線Ｌ52(k1)を境界線として上下に隣接するように設定される領域である。この場合、上側の射影変換対象領域Ｒ３u1は、その上端側の辺と下端側の辺とがそれぞれエッジ推定用領域Ｒ３uの上端側の辺、仮想的なエッジ投影線Ｌ52(k1)に一致し、且つ、横方向の範囲が、エッジ推定用領域Ｒ３uの横方向の範囲と一致する領域である。

従って、射影変換対象領域Ｒ３u1は、その縦方向の幅と下端側の辺の位置（縦方向での位置）とが、仮想的なエッジ投影線Ｌ52(k1)の位置（＝ｖ(k1)）に応じて変化するように設定される領域である。この射影変換対象領域Ｒ３u1は、仮想的なエッジ投影線Ｌ52(k1)が、実エッジ投影線Ｌ52rに合致する場合には、上段側踏面投影領域ＡＲ２ｐに含まれる領域である。

また、下側の射影変換対象領域Ｒ３u2は、その上端側の辺が仮想的なエッジ投影線Ｌ52(k1)に一致すると共に、下端側の辺が仮想的なエッジ投影線Ｌ52(k1)とあらかじめ定められた所定間隔Δｖu2（例えば１５画素分の間隔）を有して該エッジ投影線Ｌ52(k1)に平行な線に一致し、且つ、横方向の範囲が、エッジ推定用領域Ｒ３uの横方向の範囲と一致する領域である。

従って、射影変換対象領域Ｒ３u2は、その上端側の辺の位置と下端側の辺の位置とが、それらの間の間隔（すなわちＲ３u2の縦方向の幅）を一定の間隔Δｖu2に保持しつつ、仮想的なエッジ投影線Ｌ52(k1)の位置（＝ｖ(k1)）に応じて変化するように設定される領域である。この射影変換対象領域Ｒ３u2は、仮想的なエッジ投影線Ｌ52(k1)が、実エッジ投影線Ｌ52rに合致する場合には、段鼻の先端面や蹴り込み面のように、昇り階段５０の上段側踏面５１ｐの先端側で該上段側踏面５１ｐに対して起立した姿勢で形成される平面部分が投影される領域である。

また、射影変換対象領域Ｒ３u3は、上記射影変換領域Ｒ３u2よりも下側で、エッジ推定用領域Ｒ３uの下部に設定される方形状の領域である。この射影変換領域Ｒ３u3は、その下端側の辺がエッジ推定用領域Ｒ３uの下端側の辺に一致し、且つ、横方向の範囲が、エッジ推定用領域Ｒ３uの横方向の範囲と一致する領域である。

そして、射影変換対象領域Ｒ３u3の上端側の辺の位置は、本実施形態では、仮想的なエッジ投影線Ｌ52(k1)の位置（＝ｖ(k1)）に応じて次のように設定される。

すなわち、エッジ投影線推定処理部１３は、まず、上段側踏面投影領域ＡＲ２ｐに対応する平面パラメータベクトル↑ｍ2pを用いて、仮想的なエッジ投影線Ｌ52(k1)に対応する上段側踏面５１ｐの先端側エッジ線（先端側エッジ５２を包含する空間的な直線）を求める。この先端側エッジ線は、基準カメラ３の光学中心Ｃと基準画像の撮像面上の仮想的なエッジ投影線Ｌ52(k1)とを包含する平面と、上段側踏面投影領域ＡＲ２ｐに対応する平面パラメータベクトル↑ｍ2pにより規定される平面（上段側踏面５１ｐを包含する平面）との交線として算出される。

そして、エッジ投影線推定処理部１３は、上記の如く求めた先端側エッジ線を含み、且つ、上段側踏面投影領域ＡＲ２ｐに対応する平面パラメータベクトル↑ｍ2pにより規定される平面に対して直交する平面（あるいは↑ｍ2pにより規定される平面に対して、９０度に近い既定の角度で交差する平面）を決定する。この平面は、階段の段鼻の先端面又は蹴り込み面に相当する仮想的な平面（以降、起立平面ということがある）である。

さらに、エッジ投影線推定処理部１３は、上記の如く決定した起立平面と、下段側踏面投影領域ＡＲ２ｑに対応する平面パラメータベクトル↑ｍ2qにより規定される平面との交線を、基準画像に投影してなる線に、射影変換対象領域Ｒ３u3の上端側の辺が合致するように該上端側の辺の位置を設定する。

従って、射影変換対象領域Ｒ３u3の上端側の辺は、下段側踏面５１ｑの基端側エッジ５３が、上段側踏面５１ｐの先端側エッジ５２のほぼ直下に存在するものと仮定した場合における該基端側エッジ５３を基準画像に投影してなる線に相当するものとなる。この射影変換対象領域Ｒ３u3は、仮想的なエッジ投影線Ｌ52(k1)が、実エッジ投影線Ｌ52rに合致する場合には、下段側踏面投影領域ＡＲ２ｑに含まれる領域である。

なお、前記平面投影領域抽出部１２の処理によって、階段の段鼻の先端面又は蹴り込み面に相当する段差部の平面部分に対応する平面投影領域を抽出するようにした場合には、その平面投影領域に対応する平面パラメータベクトルにより規定される平面を上記起立平面として使用して、射影変換対象領域Ｒ３u3の上端の辺の位置を決定するようにしてもよい。

補足すると、段差部の上段側踏面の先端側で該上段側踏面に対して起立する姿勢となる平面部分は、蹴り込みの箇所に板部材が設けられていないような段差部にあっては、段鼻の先端面に相当するものとなる。従って、該平面部分の上下方向の幅は、上段側踏面と下段側踏面との間隔に比して小さなものとなる。さらに、この場合には、段差部の蹴り込みの箇所には、平面部分が存在しないこととなる。

そこで、本実施形態では、前記射影変換対象領域Ｒ３u2の縦方向の幅Δｖu2を比較的小さな幅とし、射影変換対象領域Ｒ３u2の下端の辺と、その下側の射影変換対象領域Ｒ３u3の上端の辺との間に間隔を設けるようにしている。

なお、基準画像に段鼻の先端面に相当する部分が投影されている場合、該基準画像における段鼻の画像の縦方向の幅は、基準カメラ３の光学中心Ｃから段鼻までの距離や、基準カメラ３の光軸Ｌｃに対する段鼻の方位角（基準画像の縦方向での方位角）に応じて変化する。従って、前記射影変換対象領域Ｒ３u2の縦方向の幅Δｖu2は、基準カメラ３の光学中心Ｃから段鼻の近辺に存在する部分（例えば上段側踏面投影領域ＡＲ２ｐの下端に対応する上段側踏面５１上の点）までの距離や、基準カメラ３の光軸Ｌｃに対する該部分の方位角等に応じて可変的に設定するようにしてもよい。また、カメラ３Ｒ，３Ｌの解像度や撮像画像の拡縮の倍率等に応じて射影変換対象領域Ｒ３u2の縦方向の幅Δｖu2を異ならせるようにしてもよい。

以上の如くＳＴＥＰ２５の処理を実行した後、次に、エッジ投影線推定処理部１３は、ＳＴＥＰ２６の処理を実行する。このＳＴＥＰ２６では、エッジ推定用領域Ｒ３uに設定した３つの射影変換対象領域Ｒ３u1，Ｒ３u2，Ｒ３u3のそれぞれが平面部分の投影領域であると仮定して、射影変換対象領域Ｒ３u1，Ｒ３u2，Ｒ３u3のそれぞれに対応する平面部分を包含する平面についての平面パラメータベクトル↑ｍu1，↑ｍu2，↑ｍu3を決定する。

この場合、射影変換対象領域Ｒ３u1，Ｒ３u3にそれぞれ対応する平面パラメータベクトル↑ｍu1，↑ｍu3は、それぞれ、上段側踏面投影領域ＡＲ２ｐに対応する平面パラメータベクトル↑ｍ2p、下段側踏面投影領域ＡＲ２ｑに対応する平面パラメータベクトル↑ｍ2qに一致するベクトルに決定される。

また、射影変換対象領域Ｒ３u2に対応する平面パラメータベクトル↑ｍu2は、前記ＳＴＥＰ２５の処理に関して説明した起立平面（仮想的なエッジ投影線Ｌ52(k1)に対応する先端側エッジ線を含み、上段側踏面５１ｐを包含する平面に対して直交又はほぼ直交する平面）の平面パラメータベクトル↑ｍに一致するベクトルに決定される。

なお、前記平面投影領域抽出部１２の処理によって、階段の段鼻の先端面又は蹴り込み面に相当する段差部の平面部分に対応する平面投影領域を抽出するようにした場合には、その平面投影領域に対応する平面パラメータベクトル↑ｍに、↑ｍu2を一致させるようにしてもよい。

次いで、エッジ投影線推定処理部１３は、ＳＴＥＰ２７の処理を実行する。このＳＴＥＰ２７では、エッジ投影線推定処理部１３は、各射影変換対象領域Ｒ３u1，Ｒ３u2，Ｒ３u3について、各射影変換対象領域Ｒ３u1，Ｒ３u2，Ｒ３u3の画像（基準画像）と、これに対応する参照画像中の領域の画像を射影変換した場合に得られる射影変換画像との誤差を表す誤差画像を作成する。

各射影変換対象領域Ｒ３u1，Ｒ３u2，Ｒ３u3に対応する誤差画像は次のように作成される。例えば射影変換対象領域Ｒ３u1に対応す誤差画像の作成処理を代表的に説明すると、基準画像の射影変換対象領域Ｒ３u1内の任意の位置Ｑでの画素値をＩ１(Ｑ)、基準画像内の位置Ｑを射影変換対象領域Ｒ３u1に対応する平面パラメータベクトル↑ｍu1により規定される前記式（１−１）の射影変換行列Ｐにより射影変換してなる参照画像内の位置をｆ(Ｑ)、参照画像内の位置ｆ(Ｑ)での画素値をＩ２(ｆ(Ｑ))とおく。

このとき、射影変換対象領域Ｒ３u1に対応する誤差画像は、射影変換対象領域Ｒ３u1の各位置Ｑの画素値を、Ｉ１(Ｑ)とＩ２(ｆ(Ｑ))との差の二乗値（＝(Ｉ１(Ｑ)−Ｉ２(ｆ(Ｑ)))²）又は絶対値（＝｜Ｉ１(Ｑ)−Ｉ２(ｆ(Ｑ))｜）に置き換えた画像として作成される。

射影変換対象領域Ｒ３u2，Ｒ３u3のそれぞれに対応する誤差画像についても同様である。ただし、この場合、参照画像中の位置ｆ(Ｑ)を求めるための射影変換に使用する平面パラメータベクトルは、↑ｍu1の代わりに、それぞれの領域に対応して決定された平面パラメータベクトル↑ｍu2，↑ｍu3が使用される。

次いで、エッジ投影線推定処理部１３は、ＳＴＥＰ２８，２９の処理を実行する。ＳＴＥＰ２８では、エッジ投影線推定処理部１３は、処理に用いる候補位置ｖ(k1)の番数k1を“１”だけ増加させる。そして、ＳＴＥＰ２９では、エッジ投影線推定処理部１３は、増加後のk1の値が候補位置ｖ(k1)の総個数であるＮ3aを超えたか否かを判断する。

このＳＴＥＰ２９の判断結果が否定的である場合には、エッジ投影線推定処理部１３は、増加後の第ｋ1番目の候補位置ｖ(k1)を用いて、前記ＳＴＥＰ２５〜２７の処理を実行する。

以上のようにして、Ｎ3a個の候補位置ｖ(k1)（k1＝１，２，…，Ｎ3a）のそれぞれに対応して、上記の誤差画像が作成される。

ＳＴＥＰ２９の判断結果が肯定的となった場合には、エッジ投影線推定処理部１３は、次に、ＳＴＥＰ３０の処理を実行する。このＳＴＥＰ３０では、エッジ投影線推定処理部１３は、図１５に示す如く、エッジ推定用領域Ｒ３u内に、縦方向に細長い複数（Ｎ3b個）の演算処理領域Ｒ３min(k2)（k2＝１，２，…，Ｎ3b）を設定する。各演算処理領域Ｒ３min(k2)は、基準画像の横方向の幅があらかじめ定められた所定の微小幅（例えば１画素分の幅）を有し、且つ、縦方向の範囲が、エッジ推定用領域Ｒ３uの縦方向の範囲に一致する細長方形状（短冊状）の領域である。

そして、これらの演算処理領域Ｒ３min(k2)は、本実施形態では、基準画像の横方向に順番に隣接して並ぶように配置され、その全体の領域がエッジ推定用領域Ｒ３uに合致するものとされる。

なお、演算処理領域Ｒ３min(k2)は、隣合う演算処理領域の間に若干の間隔を有するように設定してもよい。また、各演算処理領域Ｒ３min(k2)の横方向の幅は１画素分の幅よりも多少大きい幅（例えば２画素分の幅、３画素分の幅等）であってもよい。

補足すると、演算処理領域Ｒ３min(k2)（k2＝１，２，…，Ｎ3b）の全てを合成したものが、エッジ推定用領域Ｒ3uに合致するので、前記ＳＴＥＰ２３において、Ｎ3a個の候補位置ｖ(k1)（ｋ1＝１，２，…，Ｎ3a）を設定する処理は、換言すれば、各演算処理領域Ｒ３min(k2)に対してＮ3a個の候補位置ｖ(k1)（ｋ1＝１，２，…，Ｎ3a）を設定する処理であると言える。

また、前記ＳＴＥＰ２５において、ｖ(k1)に応じて射影変換対象領域Ｒ3u1，Ｒ3u2，Ｒ3u3を設定する処理は、換言すれば、各演算処理領域Ｒ３min(k2)に対して射影変換対象領域Ｒ3u1，Ｒ3u2，Ｒ3u3を設定する処理であると言える。

演算処理領域Ｒ３min(k2)を設定するＳＴＥＰ３０の処理は、ＳＴＥＰ２３やＳＴＥＰ２５よりも後で行なう必要はなく、例えば、ＳＴＰＥ２２において、演算処理領域Ｒ３min(k2)の設定処理を行ってもよい。

次いで、エッジ投影線推定処理部１３は、ＳＴＥＰ３１の処理を実行する。この処理では、各演算処理領域Ｒ３min(k2)に対応する処理を順番に実行するために、ｋ2の初期値として“１”が設定される。

次いで、エッジ投影線推定処理部１３は、ＳＴＥＰ３２の処理を実行する。このＳＴＥＰ３２では、エッジ投影線推定処理部１３は、第ｋ2番目（ｋ2は現在設定されている番数）の演算処理領域Ｒ３min(k2)におけるＮ3a個の前記候補位置ｖ(k1)（k1＝１，２，…，Ｎ3a）のそれぞれについて、実エッジ投影線Ｌ52rの位置（縦方向での位置）に対する各候補位置ｖ(k1)の適合度を表す評価関数Ｅu(k2)の値を算出する。

以降、第ｋ2番目の演算処理領域Ｒ３min(k2)における第ｋ1番目の候補位置ｖ(k1)の適合度を表す評価関数Ｅu(k2)をＥu(k2)_k1と表記する。

演算処理領域Ｒ３min(k2)における各候補位置ｖ(k1)に対応する評価関数Ｅu(k2)_k1の値は、ｖ(k1)に応じて設定された射影変換対象領域Ｒ３u1（以降、これをＲ３u1(k1)と表記する）に対応する前記誤差画像のうちの演算処理領域Ｒ３min(k2)に含まれる部分の画像から算出される誤差関数Ｄu1(k2)_k1の値と、ｖ(k1)に応じて設定された射影変換対象領域Ｒ３u2（以降、これをＲ３u2(k1)と表記する）に対応する誤差画像のうちの演算処理領域Ｒ３min(k2)に含まれる部分の画像から算出される誤差関数Ｄu2(k2)_k1の値と、ｖ(k1)に応じて設定された射影変換対象領域Ｒ３u3（以降、これをＲ３u3(k1)と表記する）に対応する誤差画像のうちの演算処理領域Ｒ３min(k2)に含まれる部分の画像から算出される誤差関数Ｄu3(k2)_k1の値とを線形結合（本実施形態では加算）してなる値として算出される。

より具体的には、ｖ(k1)に応じて設定された射影変換対象領域Ｒ３u1，Ｒ３u2，Ｒ３u3のそれぞれに対応する誤差画像の各画素位置Ｑでの画素値をそれぞれＩ1uerr(Ｑ)，Ｉ2uerr(Ｑ)，Ｉ3uerr(Ｑ)とする。また、演算処理領域Ｒ３min(k2)における射影変換対象領域Ｒ３u1(k1)，Ｒ３u2(k1)，Ｒ３u3(k1)のそれぞれのトータルの画素数をＮu1(k2)_k1，Ｎu2(k2)_k1，Ｎu3(k3)_k1とする。このとき、誤差関数Ｄu1(k2)_k1，Ｄu2(k2)_k1，Ｄu3(k2)_k1はそれぞれ、次式（３−１），（３−２），（３−３）により算出される。そして、これらのＤu1(k2)_k1，Ｄu2(k2)_k1，Ｄu3(k2)_k1の値から次式（４）によって、評価関数Ｅu(k2)_k1が算出される。

Ｄu1(k2)_k1＝（１／Ｎu1(k2)_k1）・ΣＩ1uerr(Ｑ) ……（３−１）
Ｄu2(k2)_k1＝（１／Ｎu2(k2)_k1）・ΣＩ2uerr(Ｑ) ……（３−２）
Ｄu3(k2)_k1＝（１／Ｎu3(k2)_k1）・ΣＩ3uerr(Ｑ) ……（３−３）
Ｅu(k2)_k1＝Ｄu1(k2)_k1＋Ｄu2(k2)_k1＋Ｄu3(k2)_k1 ……（４）

この場合、式（３−１）におけるΣＩ1uerr(Ｑ)は、演算処理領域Ｒ３min(k2)における射影変換対象領域Ｒ３u1(k1)（演算処理領域Ｒ３min(k2)と射影変換対象領域Ｒ３u1(k1)との重なり領域）での誤差画像の画素値Ｉ1uerr(Ｑ)の総和である。

また、式（３−２）におけるΣＩ2uerr(Ｑ)は、演算処理領域Ｒ３min(k2)における射影変換対象領域Ｒ３u2(k1)（演算処理領域Ｒ３min(k2)と射影変換対象領域Ｒ３u2(k1)との重なり領域）での誤差画像の画素値Ｉ2uerr(Ｑ)の総和である。

また、式（３−３）におけるΣＩ3uerr(Ｑ)は、演算処理領域Ｒ３min(k2)における射影変換対象領域Ｒ３u3(k1)（演算処理領域Ｒ３min(k2)と射影変換対象領域Ｒ３u3(k1)との重なり領域）での誤差画像の画素値Ｉ3uerr(Ｑ)の総和である。

ＳＴＥＰ３２では、以上のようにして、第ｋ2番目の演算処理領域Ｒ３min(k2)において、Ｎ3a個の候補位置ｖ(k1)（k1＝１，２，…，Ｎ3a）のそれぞれの適合度（実エッジ投影線Ｌ52rの位置としての妥当性の度合い）をそれぞれ表すＮ3a個の評価関数Ｅu(k2)_k1（k1＝１，２，…，Ｎ3a）の値が算出される。これらの評価関数Ｅu(k2)_k1（k1＝１，２，…，Ｎ3a）は、第ｋ2番目の演算処理領域Ｒ３min(k2)において、基準画像の縦方向での位置と、その位置の適合度（第ｋ2番目の演算処理領域Ｒ３min(k2)での実エッジ投影線Ｌ52rの位置に対する適合度）との間の関係を表すデータとしての意味を持つ。

次いで、エッジ投影線推定処理部１３は、ＳＴＥＰ３３，３４の処理を実行する。ＳＴＥＰ３３では、エッジ投影線推定処理部１３は、処理対象とする演算処理領域Ｒ３min(k2)の番数k2を“１”だけ増加させる。そして、ＳＴＥＰ３４では、エッジ投影線推定処理部１３は、増加後のk2の値が演算処理領域Ｒ３min(k2)の総個数であるＮ3bを超えたか否かを判断する。

このＳＴＥＰ３４の判断結果が否定的である場合には、エッジ投影線推定処理部１３は、増加後の第ｋ2番目の演算処理領域Ｒ３min(k2)に関して、前記ＳＴＥＰ３２の処理を実行する。

以上のようにして、Ｎ3a個の候補位置ｖ(k1)（k1＝１，２，…，Ｎ3a）のそれぞれと、Ｎ3b個の演算処理領域Ｒ３min(k2)（k2＝１，２，…，Ｎ3b）のそれぞれとの各組に対応して、総計Ｎ3a×Ｎ3b個の評価関数Ｅu(k2)_k1の値が算出される。

これにより、図１６（ａ），（ｂ）に例示する如く、各演算処理領域Ｒ３min(k2)毎に、基準画像の縦方向（ｖ軸方向）での所定の刻み間隔毎の複数の候補位置ｖ(k1)（ｋ1＝１，２，…，Ｎ3a）と、その各候補位置ｖ(k1)の適合度（第ｋ2番目の演算処理領域Ｒ３min(k2)での実エッジ投影線Ｌ52rの位置に対する適合度）を表す評価関数Ｅu(k2)_k1の値との間の関係を表すデータが得られることとなる。

なお、図１６（ａ），（ｂ）は、それぞれ、第１番目の演算処理領域Ｒ３min(1)に関する評価関数Ｅu(1)のグラフ、第Ｎ3b番目の演算処理領域Ｒ３min(N3b)に関する評価関数Ｅu(N3b)のグラフを代表的に示している。

ＳＴＥＰ３４の判断結果が肯定的となった場合には、エッジ投影線推定処理部１３は、次に、ＳＴＥＰ３５の処理を実行する。このＳＴＥＰ３５では、エッジ投影線推定処理部１３は、エッジ推定用領域Ｒ３uに推定エッジ投影線の複数の候補（以降、エッジ投影線候補という）を仮想的に設定する。そして、エッジ投影線推定処理部１３は、その各エッジ投影線候補毎に、Ｎ3b個の演算処理領域Ｒ３min(k2)（k2＝１，２，…，Ｎ3b）のそれぞれにおける評価関数Ｅu(k2)の値の総和ＴＥu（以降、総合評価関数ＴＥuという）を求める。

このＳＴＥＰ３５の処理でエッジ投影線推定処理部１３が仮想的に設定する複数のエッジ投影線候補は、複数種類の位置及び方向を有するエッジ投影線である。これらのエッジ投影線候補は、具体的には、例えば次のように設定される。

すなわち、図１７を参照して、エッジ投影線推定処理部１３は、Ｎ3b個の演算処理領域Ｒ３min(k2)を合成した領域としてエッジ推定用領域Ｒ３uの左端の辺と右端の辺とにそれぞれ同数ずつ、複数個の点を設定する。例えば、前記Ｎ3a個の候補位置ｖ(k1)（ｋ1＝１，２，…，Ｎ3a）、エッジ推定用領域Ｒ３uの左端の辺と右端の辺とにそれぞれ、Ｎ3a個の点を等間隔で設定する。

そして、エッジ投影線推定処理部１３は、エッジ推定用領域Ｒ３uの左端の各設定点と、右端の各設定点とを結ぶ、総計Ｎ3a×Ｎ3a個の直線をエッジ投影線候補として設定する。

図１７に示すエッジ投影線候補は、このようにして設定されるエッジ投影線候補のうちの一部を代表的に示したものである。

補足すると、エッジ推定用領域Ｒ３uの左端の辺と右端の辺とにそれぞれ設定する点の個数は、候補位置ｖ(k1)の個数Ｎ3aと同じでなくてもよく、例えばＮ3aよりも少ない個数であってもよい。また、エッジ推定用領域Ｒ３uの左端の辺と右端の辺とにそれぞれ設定する点の位置（縦方向での位置）は、ｖ(1)とｖ(N3a)との間の範囲内の位置であれば、候補位置ｖ(k1)と異なる位置であってもよい。

また、エッジ投影線候補が通る基準画像上の１つの点の位置と、該エッジ投影線候補の方向（傾斜角度）との組み合わせによって、複数のエッジ投影線候補を設定するようにしてもよい。

上記の如く設定される各エッジ投影線候補に対応する総合評価関数ＴＥuの値は、次のように算出される。

すなわち、エッジ投影線推定処理部１３は、まず、エッジ投影線候補のそれぞれについて、該エッジ投影線候補がＮ3b個の演算処理領域Ｒ３min(k2)（ｋ2＝１，２，…，Ｎ3b）のそれぞれと交差する点の、基準画像の縦方向での位置（各演算処理領域Ｒ３min(k2)での該エッジ投影線候補の位置）を算出する。

そして、エッジ投影線推定処理部１３は、エッジ投影線候補のそれぞれについて、各演算処理領域Ｒ３min(k2)での評価関数Ｅu(k2)の値を、上記の如く算出した該演算処理領域Ｒ３min(k2)でのエッジ候補線候補の位置と、該演算処理領域Ｒ３min(k2)において前記ＳＴＥＰ３２の処理により算出したＮ3a個の評価関数Ｅu(k2)_1〜Ｅu(k2)_N3aとに基づいて決定する。

具体的には、任意の１つのエッジ投影線候補に関し、例えば第ｋ2番目の演算処理領域Ｒ３min(k2)における該エッジ投影線候補の位置（ｖ軸方向での位置）がｖxであり、Ｎ3a個の前記候補位置ｖ(1)〜ｖ(N3a)のうちのｖxに一致する候補位置がｖ(k1x)であるとする。この場合、第ｋ2番目の演算処理領域Ｒ３min(k2)と候補位置ｖ(k1x)とに対応して算出された評価関数Ｅu(k2)_k1xの値が、第ｋ2番目の演算処理領域Ｒ３min(k2)での該エッジ投影線候補の評価関数Ｅu(k2)の値として決定される。

なお、ｖxに一致する候補位置が存在しない場合には、例えばｖxに最も近い候補位置を上記ｖ(k1x)として、第ｋ2番目の演算処理領域Ｒ３min(k2)での該エッジ投影線候補の評価関数Ｅu(k2)の値が決定される。

あるいは、ｖxの前後の２つ以上の候補位置のそれぞれに対応する評価関数Ｅu(k2)の値から、線形補間等の補間処理によって、ｖxに対応する評価関数Ｅu(k2)の値を算出するようにしてもよい。このようにした場合には、その算出した評価関数Ｅu(k2)の値を、第ｋ2番目の演算処理領域Ｒ３min(k2)での該エッジ投影線候補の評価関数Ｅu(k2)の値として決定するようにすればよい。

次いで、エッジ投影線推定処理部１３は、各エッジ投影線候補毎に、Ｎ3b個の演算処理領域Ｒ３min(k2)（ｋ2＝１，２，…，Ｎ3b）のそれぞれにおける該エッジ投影線候補の位置に関する評価関数Ｅu(1)〜Ｅu(N3b)の値を加え合わせることによって、該エッジ投影線候補に対応する総合評価関数ＴＥuの値を算出する。

以上の如くＳＴＥＰ３５の処理を実行した後、次に、エッジ投影線推定処理部１３は、ＳＴＥＰ３６の処理を実行する。このＳＴＥＰ３６では、エッジ投影線推定処理部１３は、ＳＴＥＰ３５で算出した各エッジ投影線候補毎の総合評価関数ＴＥuに基づいて、実エッジ投影線Ｌ52rを推定した線としての推定エッジ投影線を決定する。

ここで、ある１つのエッジ投影線候補が、実エッジ投影線Ｌ52rに合致する場合、原理的には、各演算処理領域Ｒ３min(k2)での該エッジ投影線候補の位置に関する評価関数Ｅu(k2)の値は最小（最も“０”に近い値）となる。従って、該エッジ投影線候補に対応する総合評価関数ＴＥuの値は、他のエッジ投影線候補に対応する総合評価関数ＴＥuの値よりも小さくなる。

そこで、ＳＴＥＰ３６では、エッジ投影線推定処理部１３は、ＳＴＥＰ３５で設定した全てのエッジ投影線候補のうち、対応する総合評価関数ＴＥuの値が最小となるエッジ投影線候補を、推定エッジ投影線として決定する。

これにより例えば、図１８に例示する如く、エッジ推定用領域Ｒ３uの左端の辺上の位置ｖ(k1a)の点と、右端の辺上の位置ｖ(k1b)（図示例ではｖ(k1b)＝ｖ(k1a)）の点とを結ぶエッジ投影線候補が、推定エッジ投影線として決定される。

以上が、カメラ３Ｒ，３Ｌの撮像領域に昇りの段差部（昇り階段５０）が存在する場合に実行されるエッジ投影線推定処理部１３の処理である。

次に、カメラ３Ｒ，３Ｌの撮像領域に降りの段差部（例えば降り階段５０）が存在する場合に実行されるエッジ投影線推定処理部１３の処理（図１２のＳＴＥＰ３７〜５１の処理）を説明する。

段差部が降りの段差部である場合には、エッジ投影線推定処理部１３は、まず、ＳＴＥＰ３７において、基準画像内に、ＳＴＥＰ２２と同様に、実エッジ投影線の推定用の画像処理領域であるエッジ推定用領域Ｒ３dを設定する。

ここで、以降の説明では、説明の便宜上、カメラ３Ｒ，３Ｌの撮像領域に存在する降りの段差部が、図１（ｂ）に示した如き降り階段５０であるとする。さらに、この降り階段５０の任意の１つの上段側踏面５１（例えばロボット１から見て最も手前側の上段側踏面）の先端側エッジ５２を基準画像に投影してなる実エッジ投影線Ｌ52rを推定する処理をエッジ投影線推定処理部１３により実行するものとする。

そして、昇りの段差部に関する説明と同様に、推定しようとする実エッジ投影線Ｌ52rに対応する先端側エッジ５２を先端に有する上段側踏面５１と、その一段下側の下段側踏面５１とを、それぞれ参照符号５１ｐ，５１ｑを使用して表記する。

ＳＴＥＰ３７において基準画像に設定するエッジ推定用領域Ｒ３ｄは、図１９に示す如く設定される方形状の領域である。このエッジ推定用領域Ｒ３dは、その下端の位置が上段側踏面投影領域ＡＲ２ｐの縦方向の幅内の位置（例えば上段側踏面投影領域ＡＲ２ｐの上端から既定の所定量だけ下側の位置）となり、且つ、Ｒ３dの上端の位置が下段側踏面投影領域ＡＲ２ｑの縦方向の幅内の位置（例えば下段側踏面投影領域ＡＲ２ｑの下端から所定量だけ上側の位置）になるように配置される。この場合、エッジ推定用領域Ｒ３dの縦方向の幅は、下段側踏面投影領域ＡＲｑと上段側踏面投影領域ＡＲｐとの間の縦方向の間隔に応じて決定される。

また、エッジ推定用領域Ｒ３dは、その横方向の範囲（左端と右端との間の範囲）が、下段側踏面投影領域ＡＲ２ｑ及び上段側踏面投影領域ＡＲ２ｐの横方向の範囲に一致するように配置される。この場合、エッジ推定用領域Ｒ３dの横方向の幅は、下段側踏面投影領域ＡＲ２ｑ及び上段側踏面投影領域ＡＲ２ｐの横方向の幅と同じ幅（既定の幅）に決定される。

なお、エッジ推定用領域Ｒ３dの横方向の幅は、下段側踏面投影領域ＡＲ２ｑ及び上段側踏面投影領域ＡＲ２ｐの横方向の幅よりも若干小さくてもよい。その場合は、エッジ推定用領域Ｒ３dは、その横方向の範囲が、下段側踏面投影領域ＡＲ２ｑ及び上段側踏面投影領域ＡＲ２ｐの横方向の範囲内に収まるように配置すればよい。

以上のＳＴＥＰ３７の処理によって、エッジ推定用領域Ｒ３dは、その内部に（上端の辺と下端の辺との間）に推定しようとする単一の実エッジ投影線Ｌ52rが存在し、且つ、該エッジ推定用領域Ｒ３dの上端の辺及び下端の辺がそれぞれ、下段側踏面投影領域ＡＲ２ｑ、上段側踏面投影領域ＡＲ２ｐに含まれるようにして、基準画像内に設定されることとなる。

エッジ投影線推定処理部１３は、ＳＴＥＰ３７の次にＳＴＥＰ３８の処理を実行する。このＳＴＥＰ３８では、エッジ投影線推定処理部１３は、前記ＳＴＥＰ２３と同様に処理によって、図２０に示す如く、基準画像の縦方向（ｖ軸方向）における実エッジ投影線Ｌ52rの位置の候補としての複数（Ｎ3a個）の候補位置ｖ(k1)（ｋ1＝１，２，…，Ｎ3a）を決定する。

なお、この場合、候補位置ｖ(k1)の個数Ｎ3aは、前記ＳＴＥＰ２３の処理における個数Ｎ3aと一般には異なる。

また、この場合、Ｎ3a個の候補位置ｖ(k1)（ｋ1＝１，２，…，Ｎ3a）は、エッジ推定用領域Ｒ３dの縦方向の範囲（上端と下端との間の範囲）内で、あらかじめ定められた所定の刻み間隔ずつ（例えば１画素分の間隔ずつ）、基準画像の縦方向に異なる位置とされる。そして、これらの候補位置ｖ(k1)のうち、最も上側の候補位置としての第１番目の候補位置ｖ(1)は、エッジ推定用領域Ｒ３ｄの上端から所定間隔（例えば１画素分の間隔）だけ下側の位置、最も下側の候補位置としての第Ｎ3a番目の候補位置ｖ(Ｎ3a)は、エッジ推定用領域Ｒ３ｄの下端から所定間隔（例えば１画素分の間隔）だけ上側の位置に決定される。

次に、エッジ投影線推定処理部１３は、ＳＴＥＰ３９の処理を実行する。この処理は、前記ＳＴＥＰ２４の処理と同じであり、k1の初期値として“１”が設定される。

次いで、エッジ投影線推定処理部１３は、ＳＴＥＰ４０の処理を実行する。このＳＴＥＰ４０では、エッジ投影線推定処理部１３は、基準画像の縦方向での実エッジ投影線Ｌ52rの位置が第ｋ1番目の候補位置ｖ(k1)（ｋ1は現在設定されている値）に一致するものと仮定して、エッジ推定用領域Ｒ３d内に、図２０に例示する如く、２つの射影変換対象領域Ｒ３d1，Ｒ３d2を設定する。

カメラ３Ｒ，３Ｌの撮像領域に存在する段差部が降りの段差部（ここでは降り階段５０）である場合には、該段差部の段鼻の先端面や蹴り込み面は、基準画像及び参照画像には写らない。このため、この場合にエッジ推定用領域Ｒ３dに設定する射影変換対象領域は、Ｒ３d1，Ｒ３d2の２つである。

これらの２つの射影変換対象領域Ｒ３d1，Ｒ３d2は、より詳しくは、第ｋ1番目の候補位置ｖ(k1)で横方向に延在する仮想的なエッジ投影線Ｌ52(k1)を境界線として上下に隣接するように設定される領域である。この場合、上側の射影変換対象領域Ｒ３d1は、その上端側の辺と下端側の辺とがそれぞれエッジ推定用領域Ｒ３dの上端側の辺、仮想的なエッジ投影線Ｌ52(k1)に一致し、且つ、横方向の範囲が、エッジ推定用領域Ｒ３dの横方向の範囲と一致する領域である。

従って、射影変換対象領域Ｒ３d1は、その縦方向の幅と下端側の辺の位置（縦方向での位置）とが、仮想的なエッジ投影線Ｌ52(k1)の位置（＝ｖ(k1)）に応じて変化するように設定される領域である。この射影変換対象領域Ｒ３d1は、仮想的なエッジ投影線Ｌ52(k1)が、実エッジ投影線Ｌ52rに合致する場合には、下段側踏面投影領域ＡＲ２ｑに含まれる領域である。

また、下側の射影変換対象領域Ｒ３d2は、その上端側の辺と下端側の辺とがそれぞれ仮想的なエッジ投影線Ｌ52(k1)、エッジ推定用領域Ｒ３dの下端側の辺に一致し、且つ、横方向の範囲が、エッジ推定用領域Ｒ３dの横方向の範囲と一致する領域である。

従って、射影変換対象領域Ｒ３d2は、その縦方向の幅と上端側の辺の位置（縦方向での位置）とが、仮想的なエッジ投影線Ｌ52(k1)の位置（＝ｖ(k1)）に応じて変化するように設定される領域である。この射影変換対象領域Ｒ３d2は、仮想的なエッジ投影線Ｌ52(k1)が、実エッジ投影線Ｌ52rに合致する場合には、上段側踏面投影領域ＡＲ２ｐに含まれる領域である。

以上の如くＳＴＥＰ４０の処理を実行した後、次に、エッジ投影線推定処理部１３は、ＳＴＥＰ４１の処理を実行する。このＳＴＥＰ４１では、エッジ推定用領域Ｒ３dに設定した２つの射影変換対象領域Ｒ３d1，Ｒ３d2のそれぞれが平面部分の投影領域であると仮定して、射影変換対象領域Ｒ３d1，Ｒ３d2のそれぞれに対応する平面部分を包含する平面についての平面パラメータベクトル↑ｍd1，↑ｍd2を決定する。

この場合、射影変換対象領域Ｒ３d1，Ｒ３d2にそれぞれ対応する平面パラメータベクトル↑ｍd1，↑ｍd2は、それぞれ、下段側踏面投影領域ＡＲ２ｑに対応する平面パラメータベクトル↑ｍ2q、上段側踏面投影領域ＡＲ２ｐに対応する平面パラメータベクトル↑ｍ2pに一致するベクトルに決定される。

次いで、エッジ投影線推定処理部１３は、ＳＴＥＰ４２の処理を実行する。このＳＴＥＰ４２では、エッジ投影線推定処理部１３は、各射影変換対象領域Ｒ３d1，Ｒ３d2について、各射影変換対象領域Ｒ３d1，Ｒ３d2の画像（基準画像）と、これに対応する参照画像中の領域の画像を射影変換した場合に得られる射影変換画像との誤差を表す誤差画像を作成する。

この場合、各射影変換対象領域Ｒ３d1，Ｒ３d2に対応する誤差画像は、それぞれに対応する平面パラメータベクトル↑ｍd1，↑ｍd2を使用して、前記ＳＴＥＰ２７での誤差画像の作成処理と同様の処理によって作成される。

次いで、エッジ投影線推定処理部１３は、ＳＴＥＰ４３，４４の処理を実行する。ＳＴＥＰ４３では、エッジ投影線推定処理部１３は、処理に用いる候補位置ｖ(k1)の番数k1を“１”だけ増加させる。そして、ＳＴＥＰ４４では、エッジ投影線推定処理部１３は、増加後のk1の値が候補位置ｖ(k1)の総個数であるＮ3aを超えたか否かを判断する。

このＳＴＥＰ４４の判断結果が否定的である場合には、エッジ投影線推定処理部１３は、増加後の第ｋ1番目の候補位置ｖ(k1)を用いて、前記ＳＴＥＰ４０〜４２の処理を実行する。

ＳＴＥＰ４４の判断結果が肯定的となった場合には、エッジ投影線推定処理部１３は、次に、ＳＴＥＰ４５の処理を実行する。このＳＴＥＰ４５では、エッジ投影線推定処理部１３は、図２１に示す如く、エッジ推定用領域Ｒ３d内に、縦方向に細長い複数（Ｎ3b個）の演算処理領域Ｒ３min(k2)（k2＝１，２，…，Ｎ3b）を設定する。各演算処理領域Ｒ３min(k2)（k2＝１，２，…，Ｎ3b）は、前記ＳＴＥＰ３０の処理に関して説明したものと同様の細長方形状（短冊状）の領域である。そして、その設定の仕方は、ＳＴＥＰ３０の処理と同じである。

なお、演算処理領域Ｒ３min(k2)の個数Ｎ3bは、前記ＳＴＥＰ３０で設定する演算処理領域Ｒ３min(k2)の個数と同じでなくてもよい。

補足すると、前記ＳＴＥＰ３０に関して説明したことと同様に、前記ＳＴＥＰ３８において、Ｎ3a個の候補位置ｖ(k1)（ｋ1＝１，２，…，Ｎ3a）を設定する処理は、換言すれば、各演算処理領域Ｒ３min(k2)に対してＮ3a個の候補位置ｖ(k1)（ｋ1＝１，２，…，Ｎ3a）を設定する処理であると言える。

また、前記ＳＴＥＰ４０において、ｖ(k1)に応じて射影変換対象領域Ｒ3d1，Ｒ3d2を設定する処理は、換言すれば、各演算処理領域Ｒ３min(k2)に対して射影変換対象領域Ｒ3d1，Ｒ3d2を設定する処理であると言える。

ＳＴＥＰ４５の処理は、ＳＴＥＰ３８やＳＴＥＰ４０よりも後で行なう必要はなく、例えば、ＳＴＥＰ３７において、演算処理領域Ｒ３min(k2)の設定処理を行ってもよい。

次いで、エッジ投影線推定処理部１３は、ＳＴＥＰ４６の処理を実行する。この処理では、ｋ2の初期値として“１”が設定される。

次いで、エッジ投影線推定処理部１３は、ＳＴＥＰ４７の処理を実行する。このＳＴＥＰ４７では、エッジ投影線推定処理部１３は、第ｋ2番目（ｋ2は現在設定されている番数）の演算処理領域Ｒ３min(k2)におけるＮ3a個の前記候補位置ｖ(k1)（k1＝１，２，…，Ｎ3a）のそれぞれについて、実エッジ投影線Ｌ52rの位置（縦方向での位置）に対する各候補位置ｖ(k1)の適合度を表す評価関数Ｅd(k2)の値を、前記ＳＴＥＰ３２の処理の場合と同様の処理によって算出する。

ただし、この場合、エッジ推定用領域Ｒ３dに設定される射影変換対象領域はＲ３d1，Ｒ３d2の２つであるので、演算処理領域Ｒ３min(k2)における各候補位置ｖ(k1)に対応する評価関数Ｅd(k2)_k1の値は、次のように算出される。

すなわち、演算処理領域Ｒ３min(k2)における各候補位置ｖ(k1)に対応する評価関数Ｅd(k2)_k1の値は、ｖ(k1)に応じて設定された射影変換対象領域Ｒ３d1（以降、これをＲ３d1(k1)と表記する）に対応する前記誤差画像のうちの演算処理領域Ｒ３min(k2)に含まれる部分の画像から算出される誤差関数Ｄd1(k2)_k1の値と、ｖ(k1)に応じて設定された射影変換対象領域Ｒ３d2（以降、これをＲ３d2(k1)と表記する）に対応する誤差画像のうちの演算処理領域Ｒ３min(k2)に含まれる部分の画像から算出される誤差関数Ｄd2(k2)_k1の値とを線形結合（本実施形態では加算）してなる値として算出される。

さらに詳細には、ｖ(k1)に応じて設定された射影変換対象領域Ｒ３d1，Ｒ３d2のそれぞれに対応する誤差画像の各画素位置Ｑでの画素値をそれぞれＩ1derr(Ｑ)，Ｉ2derr(Ｑ)，Ｉ3derr(Ｑ)とする。また、演算処理領域Ｒ３min(k2)における射影変換対象領域Ｒ３d1(k1)，Ｒ３d2(k1)のそれぞれのトータルの画素数をＮd1(k2)_k1，Ｎd2(k2)_k1とする。このとき、誤差関数Ｄd1(k2)_k1，Ｄd2(k2)_k1はそれぞれ、次式（５−１），（５−２）により算出される。そして、これらのＤd1(k2)_k1，Ｄd2(k2)_k1の値から次式（６）によって、評価関数Ｅd(k2)_k1が算出される。

Ｄd1(k2)_k1＝（１／Ｎd1(k2)_k1）・ΣＩ1derr(Ｑ) ……（５−１）
Ｄd2(k2)_k1＝（１／Ｎd2(k2)_k1）・ΣＩ2derr(Ｑ) ……（５−２）
Ｅd(k2)_k1＝Ｄd1(k2)_k1＋Ｄd2(k2)_k1 ……（６）

この場合、式（５−１）におけるΣＩ1derr(Ｑ)は、演算処理領域Ｒ３min(k2)における射影変換対象領域Ｒ３d1(k1)（演算処理領域Ｒ３min(k2)と射影変換対象領域Ｒ３d1(k1)との重なり領域）での誤差画像の画素値Ｉ1derr(Ｑ)の総和である。

また、式（３−２）におけるΣＩ2derr(Ｑ)は、演算処理領域Ｒ３min(k2)における射影変換対象領域Ｒ３u2(k1)（演算処理領域Ｒ３min(k2)と射影変換対象領域Ｒ３u2(k1)との重なり領域）での誤差画像の画素値Ｉ2derr(Ｑ)の総和である。

ＳＴＥＰ４７では、以上のようにして、第ｋ2番目の演算処理領域Ｒ３min(k2)において、Ｎ3a個の候補位置ｖ(k1)（k1＝１，２，…，Ｎ3a）のそれぞれの適合度（実エッジ投影線Ｌ52rの位置としての妥当性の度合い）をそれぞれ表すＮ3a個の評価関数Ｅd(k2)_k1（k1＝１，２，…，Ｎ3a）の値が算出される。

次いで、エッジ投影線推定処理部１３は、ＳＴＥＰ４８，４９の処理を実行する。ＳＴＥＰ４８では、エッジ投影線推定処理部１３は、処理対象とする演算処理領域Ｒ３min(k2)の番数k2を“１”だけ増加させる。そして、ＳＴＥＰ４９では、エッジ投影線推定処理部１３は、増加後のk2の値が演算処理領域Ｒ３min(k2)の総個数であるＮ3bを超えたか否かを判断する。

このＳＴＥＰ４９の判断結果が否定的である場合には、エッジ投影線推定処理部１３は、増加後の第ｋ2番目の演算処理領域Ｒ３min(k2)に関して、前記ＳＴＥＰ４７の処理を実行する。

以上のようにして、Ｎ3a個の候補位置ｖ(k1)（k1＝１，２，…，Ｎ3a）のそれぞれと、Ｎ3b個の演算処理領域Ｒ３min(k2)（k2＝１，２，…，Ｎ3b）のそれぞれとの各組に対応して、総計Ｎ3a×Ｎ3b個の評価関数Ｅd(k2)_k1の値が算出される。

これにより、各演算処理領域Ｒ３min(k2)毎に、基準画像の縦方向（ｖ軸方向）での所定の刻み間隔毎の複数の候補位置ｖ(k1)（ｋ1＝１，２，…，Ｎ3a）と、その各候補位置ｖ(k1)の適合度（第ｋ2番目の演算処理領域Ｒ３min(k2)での実エッジ投影線Ｌ52rの位置に対する適合度）を表す評価関数Ｅd(k2)_k1の値との間の関係を表すデータが得られることとなる。

ＳＴＥＰ４９の判断結果が肯定的となった場合には、エッジ投影線推定処理部１３は、次に、ＳＴＥＰ５０の処理を実行する。このＳＴＥＰ５０では、エッジ投影線推定処理部１３は、前記ＳＴＥＰ３５の処理と同様に、エッジ推定用領域Ｒ３dに複数のエッジ投影線候補を仮想的に設定する。そして、エッジ投影線推定処理部１３は、その各エッジ投影線候補毎に、Ｎ3b個の演算処理領域Ｒ３min(k2)（k2＝１，２，…，Ｎ3b）のそれぞれにおける評価関数Ｅd(k2)の値の総和である総合評価関数ＴＥdを求める。

このＳＴＥＰ５０における複数のエッジ投影線候補の設定処理と、各エッジ投影線候補に対応する総合評価関数ＴＥdの値の算出処理とは、前記ＳＴＥＰ３５の処理と同様の処理によって行われる。

次に、エッジ投影線推定処理部１３は、ＳＴＥＰ５１の処理を実行する。このＳＴＥＰ５１の処理は、前記ＳＴＥＰ３６の処理と同様に行なわれる。すなわち、エッジ投影線推定処理部１３は、ＳＴＥＰ５０で設定した全てのエッジ投影線候補のうち、対応する総合評価関数ＴＥdの値が最小となるエッジ投影線候補を、推定エッジ投影線として決定する。

以上が、カメラ３Ｒ，３Ｌの撮像領域に降りの段差部（降り階段５０）が存在する場合に実行されるエッジ投影線推定処理部１３の処理である。

以上説明したエッジ投影線推定処理部１３の処理によって、カメラ３Ｒ，３Ｌの撮像領域に段差部が存在する場合に、その段差部が、ロボット１から見て、昇りの段差部、降りの段差部のいずれであっても、該段差部の上段側踏面の先端側エッジを基準画像に投影してなる実エッジ投影線が推定される。

なお、以上のエッジ投影線推定処理部１３の処理の説明では、ＳＴＥＰ２２〜３６の処理、又は、ＳＴＥＰ３７〜５１の処理によって、任意の１つの先端側エッジ５２に対応する推定エッジ投影線を求める場合について説明したが、基準画像に投影されている各上段側踏面５１の先端側エッジ５２毎に、ＳＴＥＰ２２〜３６の処理、又は、ＳＴＥＰ３７〜５１の処理を繰り返すことで、基準画像に写っている各先端側エッジ５２に対応する推定エッジ投影線をそれぞれ求めることができる。

このエッジ投影線推定処理部１３の処理が終了すると、演算処理装置１０は次に、段差部空間配置決定部１４の処理を実行する。

この段差部空間配置決定部１４は、平面投影領域抽出部１２の処理によって抽出された各平面投影領域ＡＲ２の平面パラメータベクトル↑ｍ2と、エッジ投影線推定処理部１３の処理によって求められた推定エッジ投影線とを用いて、カメラ３Ｒ，３Ｌにより撮像された段差部の空間的な配置位置（ロボット座標系で見た配置位置）を決定する。

この場合、本実施形態では、段差部空間配置決定部１４は、段差部の空間的な配置を示すデータとして、例えば、ロボット１の空間的な進行予定領域での段差部の各上段側踏面の先端側エッジ及び各下段側踏面の基端側エッジの空間的な配置位置、並びに、各踏面の空間的な姿勢（法線方向）を決定する。

これらのデータは、基準画像上で決定した推定エッジ投影線と、この推定エッジ投影線に対応する先端側エッジを有する上段側踏面の平面パラメータベクトル↑ｍ2（↑ｍ2p）と、該上段側踏面の一段下側の下段側踏面の平面パラメータベクトル↑ｍ2（↑ｍ2q）とを使用して決定される。

具体的には、段差部空間配置決定部１４は、基準画像における推定エッジ投影線を、推定エッジ投影線に対応する先端側エッジを有する上段側踏面の平面パラメータベクトル↑ｍ2pにより規定される平面に逆投影してなる線と、該先端側エッジに合致する線として決定する。その線は、基準画像の撮像面における推定エッジ投影線と基準カメラ３の光学中心Ｃとを含む平面と、上記平面パラメータベクトル↑ｍ2pにより規定される平面との交線上の線として決定される。

また、段差部空間配置決定部１４は、上記の如く空間的な配置を決定した先端側エッジを、下段側踏面の平面パラメータベクトル↑ｍ2qにより規定される平面に所定の方向で投影してなる線を該下段側踏面の基端側エッジとして決定する。この場合、上記所定の方向は、具体的には、先端側エッジを有する上段側踏面の平面パラメータベクトル↑ｍ2pにより規定される平面に直交する方向、あるいは↑ｍ2pにより規定される平面に対して、９０度に近い既定の角度で交差する方向である。

なお、前記平面投影領域抽出部１２の処理によって、階段の段鼻の先端面又は蹴り込み面に相当する段差部の平面部分に対応する平面投影領域を抽出するようにした場合には、その平面投影領域に対応する平面パラメータベクトルにより規定される平面の法線方向に、先端側エッジと基端側エッジとを含む平面の法線方向が一致もしくはほぼ一致するように、上記所定の方向を決定してもよい。

また、段差部空間配置決定部１４は、段差部の上段側踏面及び下段側踏面の姿勢（法線方向）を、それぞれに対応する平面パラメータベクトル↑ｍ2p、↑ｍ2qの方向と一致する方向（これは一般には、鉛直方向とは限らない）に決定する。

以上により、段差部の各上段側踏面の先端側エッジ及び各下段側踏面の基端側エッジの空間的な配置位置、並びに、各踏面の空間的な姿勢（法線方向）が決定される。

そして、ロボット１は、これらのデータによりロボット１に対する段差部の配置位置やその形状を認識しつつ、該段差部を昇降する等の動作を行なう。

以上説明した本実施形態によれば、横方向の幅が微小で、且つ縦方向に細長い複数の演算処理領域Ｒ3min(k2)（ｋ2＝１，２，…，Ｎ3b）が基準画像に横方向に並ぶようにして設定される。そして、その各演算処理領域毎に、基準画像の縦方向の複数の候補位置ｖ(k1)（ｋ1＝１，２，…，Ｎ3a）のそれぞれの適合度（実エッジ投影線Ｌ52rの位置に対する適合度）を表す評価関数Ｅu(k2)_k1又はＥd(k2)_k1が、射影変換を利用した演算処理によって算出される。

これにより、横方向に並ぶ複数の演算処理領域Ｒ3min(k2)（ｋ2＝１，２，…，Ｎ3b）のそれぞれにおいて、縦方向の位置と、評価関数Ｅu(k2)_k1又はＥd(k2)_k1の値との関係を表すデータが得られることとなる。

さらに、エッジ推定用領域Ｒ３u又はＲ３dに、位置や方向が互いに相違する複数のエッジ投影線候補を仮想的に設定され、それぞれのエッジ投影線候補に対して、各演算処理領域Ｒ3min(k2)での位置の評価関数Ｅu(k2)_k1又はＥd(k2)_k1の値を合成してなる総合評価関数ＴＥu又はＴＥdの値が算出される。この総合評価関数ＴＥu又はＴＥdは、各演算処理領域Ｒ3min(k2)でのエッジ投影線候補の位置の適合度（実エッジ投影線Ｌ52rの位置に対する適合度）を合成してなる合成適合度の逆数値に相当するものである。

そして、この総合評価関数ＴＥu又はＴＥdの値が最小となるエッジ投影線候補（換言すれば、上記合成適合度が最大となるエッジ投影線候補）が、推定エッジ投影線として決定される。

このようにして推定エッジ投影線を決定することにより、実エッジ投影線を推定したものとしての信頼性の高い推定エッジ投影線を決定できる。

また、特に、カメラ３Ｒ，３Ｌの撮像領域に昇り階段５０などの昇りの段差部が存在する場合に、候補位置ｖ(k1)に依存する３つの射影変換対象領域Ｒ3u1，Ｒ3u2，Ｒ3u1をエッジ推定用領域Ｒ３uに前記した如く設定する（ひいては各演算処理領域Ｒ３min(k2)に設定する）ので、前記各評価関数Ｅu(k2)_k1の値に、候補位置ｖ(k1)の位置を境界として隣接する射影変換対象領域Ｒ3u1，Ｒ3u2だけでなく、下段側踏面に対応する射影変換対象領域Ｒ3u3の画像の画素値を反映させることができる。すなわち、前記各評価関数Ｅu(k2)_k1の値を、各演算処理領域Ｒ３min(k2)に含まれる多くの画素の画素値を反映させて算出することができる。

そのため、各演算処理領域Ｒ３min(k2)における候補位置ｖ(k1)の位置の適合度を表すものとしての各評価関数Ｅu(k2)_k1の値の信頼性を高めることができる。ひいては、推定エッジ投影線の信頼性を高めることができる。

また、本実施形態では、段差部存在検知部１１や、平面投影領域抽出部１２は、ステレオ画像と射影変換とを利用した処理によって、レーザ測距装置等の測距装置をロボット１に備えることなく、段差部の存在を検知したり、平面投影領域の抽出を行なうことができる。このため、段差部の認識を安価な構成で行なうことができる。

ここで、以上説明した実施形態と本発明との対応関係について補足しておく。

本実施形態では、前記段差部存在検知部１１の処理によって、本発明における段差部存在検知手段が実現される。この場合、段差部存在検知部１１は、本発明における段差部種別判断手段としての機能を含んでおり、前記ＳＴＥＰ４の処理によって、段差部種別判断手段が実現される。

また、前記平面投影領域抽出部１２の処理によって、本発明における平面投影領域抽出手段が実現される。この場合、前記局所領域Ｒ２(i)（ｉ＝１，２，…，Ｎ2）が、本発明における平面投影領域抽出用局所領域に相当する。

また、前記エッジ投影線推定処理部１３の処理によって、本発明における評価関数算出手段と推定エッジ投影線決定手段とが実現される。より詳しくは、前記ＳＴＥＰ２２〜３４の処理及びＳＴＥＰ３７〜４９の処理によって、評価関数算出手段が実現され、前記ＳＴＥＰ３５，３６並びにＳＴＥＰ５０，５１の処理によって、推定エッジ投影線決定手段が実現される。

この場合、射影変換対象領域Ｒ3u1，Ｒ3u2，Ｒ3u3が、それぞれ、本発明における第１のエッジ推定用射影変換対象領域、第２のエッジ推定用射影変換対象領域、第３のエッジ推定用射影変換対象領域に相当し、射影変換対象領域Ｒ3d1，Ｒ3d2が、それぞれ、本発明における第４のエッジ推定用射影変換対象領域、第５のエッジ推定用射影変換対象領域に相当する。また、前記平面パラメータベクトル↑ｍu1，↑ｍu2，↑ｍu3，↑ｍd1，↑ｍd2がそれぞれ、本発明における射影変換用平面パラメータに相当する。

また、前記段差部空間配置決定部１４の処理によって、本発明における段差部空間配置決定手段が実現される。

次に、以上説明した実施形態の変形態様をいくつか説明しておく。

前記実施形態では、移動体として２脚の脚式移動ロボット１を例示したが、移動体は、３脚以上の脚を有するロボット、あるいは、車輪やクローラを備える移動体であってもよい。

また、前記実施形態では、カメラ３Ｒ，３Ｌのうちのカメラ３Ｒを基準カメラ３として使用したが、カメラ３Ｌを基準カメラとして使用してもよい。

また、前記実施形態では、カメラ３Ｒ，３Ｌの撮像領域に昇りの段差部が存在する場合に、候補位置ｖ(k1)に依存する３つの射影変換対象領域Ｒ3u1，Ｒ3u2，Ｒ3u3をエッジ推定用領域Ｒ３uに設定するようにしたが、射影変換対象領域Ｒ3u1，Ｒ3u2の２つの射影変換対象領域だけをエッジ推定用領域Ｒ３uに設定（ひいては各演算処理領域Ｒ３min(k2)に設定）するようにしてもよい。

また、前記実施形態では、各演算処理領域Ｒ３min(k2)における複数の候補位置は、全ての演算処理領域Ｒ３min(k2)について同じであるが、各演算処理領域Ｒ３min(k2)毎の候補位置を異ならせるようにしてもよい。

また、前記実施形態では、平面投影領域抽出部１２の処理において、複数の局所領域Ｒ２(i)（ｉ＝１，２，…，Ｎ2）の、基準画像の横方向での位置を同じとしたが、さらに、基準画像の横方向での複数の位置のそれぞれおいて、複数の局所領域Ｒ２(i)を縦方向に並べるように配置してもよい。

このようにした場合には、縦方向での平面部分の分布の変化を、基準画像の横方向の複数位置において、把握することができる。このため、カメラ３Ｒ，３Ｌの撮像領域に存在する段差部の各踏面が、基準画像の横方向に対して比較的大きく傾いているような場合に、その傾きを反映させて、段差部の各踏面等の平面部分に対応する平面投影領域を抽出することができる。従って、該平面部分の傾きに合せて、上端及び下端の辺が基準画像の横方向に対して傾斜したような形状の平面投影領域を抽出するようにすることができる。

１…脚式移動ロボット（移動体）、３Ｒ，３Ｌ…カメラ（ステレオカメラ）、４…投光器、１１…段差部存在検知部（段差部存在検知手段、段差部種別判断手段）、１２…平面投影領域抽出部（平面投影領域抽出手段）、１３…エッジ投影線推定処理部（評価関数算出手段、推定エッジ投影線決定手段）、１４…段差部空間配置決定部（段差部空間配置決定手段）、ＳＴＥＰ４…段差部種別判断手段、ＳＴＥＰ２２〜３４，ＳＴＥＰ３７〜４９…評価関数算出手段、ＳＴＥＰ３５，３６，５０，５１…推定エッジ投影線決定手段、５０…階段（段差部）、５１…踏面、５２…先端側エッジ。

Claims

ステレオカメラを搭載した移動体の移動環境に存在しており、下段側踏面とその一段上側の上段側踏面との組を一組以上有する段差部の空間的な配置位置を、前記ステレオカメラにより取得されるステレオ画像を構成する基準画像及び参照画像を用いて認識する段差部認識装置であって、
前記段差部が前記ステレオカメラにより撮像されている状態において、前記基準画像から、該段差部の上段側踏面及び下段側踏面にそれぞれ対応する２つの平面部分を少なくとも含む互いに異なる平面部分がそれぞれ投影された領域である複数の平面投影領域を抽出すると共に、各平面投影領域に対応する平面部分を包含する平面の空間的な位置及び姿勢を表す平面パラメータを決定する平面投影領域抽出手段と、
前記段差部の上段側踏面の先端側エッジを前記基準画像に投影してなる実エッジ投影線をそれぞれ含み、且つ、前記基準画像の縦方向に細長い形状を有して該基準画像の横方向に並列する複数条の演算処理領域を該基準画像に設定すると共に、該複数条の演算処理領域のそれぞれにおける前記基準画像の縦方向での前記実エッジ投影線の複数の候補位置を設定し、各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を、前記基準画像及び参照画像と、前記決定された平面パラメータとを用いて算出する評価関数算出手段と、
前記実エッジ投影線を推定してなる線である推定エッジ投影線を、前記複数条の演算処理領域のそれぞれにおける該推定エッジ投影線の位置の適合度を合成してなる合成適合度が最も高くなるように、前記算出された評価関数に基づいて決定する推定エッジ投影線決定手段と、
該段差部の空間的な配置位置を認識するためのデータとして、少なくとも前記段差部の上段側踏面の先端側エッジの空間的な位置及び方向を表すデータを前記決定された推定エッジ投影線と前記決定された平面パラメータとに基づいて決定する段差部空間配置決定手段とを備え、
前記評価関数算出手段は、前記各演算処理領域毎に、各候補位置の実エッジ投影線の位置に対する適合度を表す評価関数の値を算出するために、各演算処理領域内に各候補位置を境界として隣接する２つの射影変換対象領域を少なくとも含む複数のエッジ推定用射影変換対象領域を各候補位置に応じて設定する処理と、各エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータを前記平面投影領域抽出手段により決定された平面パラメータに応じて設定する処理と、各演算処理領域及び各候補位置の組毎に、各エッジ推定用射影変換対象領域の画像の画素値分布と、該エッジ推定用射影変換対象領域に対応する前記参照画像中の領域の画像を該エッジ推定用射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す誤差関数の値を算出する処理と、各演算処理領域及び各候補位置の組毎に、各演算処理領域内に設定した複数のエッジ推定用射影変換対象領域のそれぞれに対応して算出した前記誤差関数の値を線形結合してなる値を、前記評価関数の値として算出する処理とを実行し、
前記推定エッジ投影線決定手段は、前記推定エッジ投影線の複数の候補として、前記基準画像での位置及び方向の一方又は両方が互いに異なる複数の推定エッジ投影線候補を設定する処理と、当該複数のエッジ投影線候補の各々に対して、前記複数条の演算処領域のそれぞれにおける該エッジ投影線候補の位置の適合度を表す値として前記算出された評価関数に基づいて決定した値を合成することにより各エッジ投影線候補の合成適合度を表す総合評価関数値を算出する処理と、前記複数のエッジ投影線候補のうち、当該総合評価関数値により表される合成適合度が最も高くなるエッジ投影線候補を前記推定エッジ投影線として決定する処理とを実行するように構成されていることを特徴とする段差部認識装置。
請求項１記載の段差部認識装置において、
前記平面投影領域抽出手段は、前記基準画像における位置が互いに相違する複数の平面投影領域抽出用局所領域を該基準画像に設定する処理と、各平面投影領域抽出用局所領域の画像と該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである平面投影領域抽出用平面パラメータを、該平面投影領域抽出用局所領域の画像の画素値分布と、該平面投影領域抽出用局所領域に対応する前記参照画像中の領域の画像を該平面投影領域抽出用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する処理とを実行し、各平面投影領域に含まれる前記平面投影領域抽出用局所領域のそれぞれに対応する平面投影領域抽出用平面パラメータにより規定される平面が互いに一致するように前記基準画像から各平面投影領域を抽出することを特徴とする段差部認識装置。
請求項１又は２記載の段差部認識装置において、
前記評価関数算出手段が各演算処理領域に設定する複数の候補位置は、全ての演算処理領域について共通のＮ3a個（Ｎ3a：２以上の整数）の候補位置であり、前記評価関数算出手段は、第ｋ1番目（ｋ1：１からＮ3aまでの整数のうちの任意の１つの整数）の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出するために、前記射影変換用平面パラメータが互いに同一となる各演算処理領域のエッジ推定用射影変換対象領域を合成してなる合成射影変換対象領域における画像と、該合成射影変換対象領域に対応する前記参照画像中の領域の画像を該合成射影変換対象領域に対応する射影変換用平面パラメータに応じて射影変換した場合に得られる画像との誤差画像を求める処理を実行し、その誤差画像を用いて、第ｋ1番目の候補位置と各演算処理領域とに対応する前記誤差関数の値を算出することを特徴とする段差部認識装置。
請求項１〜３のいずれか１項に記載の段差部認識装置において、
前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知する段差部存在検知手段をさらに備えており、
前記平面投影領域抽出手段、評価関数算出手段及び段差部配置決定手段の処理は、前記段差部存在検知手段によって前記ステレオカメラの撮像領域に段差部が存在していることが検知されることを必要条件として実行される処理であり、
前記段差部存在検知手段は、前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知するための段差部存在検知用領域を前記基準画像に設定する処理と、該段差部存在検知用領域の画像と該段差部存在検知用領域に対応する前記参照画像中の領域の画像との間の射影変換を規定する平面パラメータである段差部存在検知用平面パラメータを、前記段差部存在検知用領域の画像の画素値分布と該段差部存在検知用領域に対応する領域の画像を該段差部存在検知用平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を最小化するように決定する処理とを実行し、決定した該段差部存在検知用平面パラメータにより示される平面の法線方向の向きに基づいて前記ステレオカメラの撮像領域に段差部が存在しているか否かを検知することを特徴とする段差部認識装置。
請求項１〜４のいずれか１項に記載の段差部認識装置において、
前記段差部が前記ステレオカメラにより撮像されている状態において、該段差部が昇りの段差部であるか、降りの段差部であるかを判断する段差部種別判断手段をさらに備えており、
前記評価関数算出手段は、
前記段差部種別判断手段によって前記段差部が昇り段差部であると判断された場合には、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が該段差部の上段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が該段差部の下段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定し、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第１及び第２のエッジ推定用射影変換対象領域を該演算処理領域の上部側に設定すると共に、該第１及び第２のエッジ推定用射影変換対象領域のうちの下側の領域である第２のエッジ推定用射影変換対象領域との間に該候補位置に応じた間隔を有する第３のエッジ推定用射影変換対象領域を該演算処理領域の下部側に設定し、
前記段差部種別判断手段によって前記段差部が降り段差部であると判断された場合には、前記基準画像に前記複数条の演算処理領域を設定する処理において、各演算処理領域の上端が下段側踏面に対応する前記平面投影領域に位置し、且つ、各演算処理領域の下端が上段側踏面に対応する前記平面投影領域に位置するように前記複数条の演算処理領域を設定し、前記各演算処理領域内に前記複数のエッジ推定用射影変換対象領域を設定する処理において、各候補位置を境界として隣接する第４及び第５のエッジ推定用射影変換対象領域を該演算処理領域内に設定することを特徴とする段差部認識装置。
請求項５記載の段差部認識装置において、
前記評価関数算出手段は、前記段差部種別判断手段によって前記段差部が昇りの段差部であると判断された場合に、各エッジ推定用射影変換対象領域に対応するエッジ推定用平面パラメータを設定する処理において、前記第１のエッジ推定用射影変換対象領域と前記第３のエッジ推定用射影変換対象領域とのそれぞれに対応する射影変換用平面パラメータとして、それぞれ、前記段差部の上段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータと、前記段差部の下段側踏面に対応する前記平面投影領域に関して決定された前記平面パラメータとを設定し、前記第２のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータとして、第１のエッジ推定用射影変換対象領域に対応する射影変換用平面パラメータにより規定される平面に対してあらかじめ定めた角度で交差する平面の位置及び姿勢を表す平面パラメータを設定することを特徴とする段差部認識装置。
請求項１〜６のいずれか１項に記載の段差部認識装置において、
前記段差部空間配置決定手段は、前記段差部の上段側踏面の先端側エッジを、該上段側踏面に対応する前記平面パラメータにより規定される平面に対してあらかじめ定めた角度で交差する方向で該段差部の下段側踏面に対応する前記平面パラメータにより規定される平面に投影した線を、該下段側踏面の基端側の境界線として決定する手段を含むことを特徴とする段差部認識装置。
請求項１〜７のいずれか１項に記載の段差部認識装置において、
前記移動体には、前記ステレオカメラのよる撮像時に該ステレオカメラの撮像領域にテクスチャを付与する投光器が搭載されていることを特徴とする段差部認識装置。