段差エッジ推定装置
【課題】撮像手段により取得されるステレオ画像を利用して、階段等の段差部の平坦面部の先端部のエッジの位置及び方向を精度良く推定することを可能とする装置を提供する。
【解決手段】撮像手段３Ｒ，３Ｌにより撮像した２つの撮像画像（ステレオ画像）を用いて段差部（階段）５０の段差エッジ５２の位置及び方向を推定する。第１の撮像画像に設定した領域における画像の素値分布と、第２の撮像画像の対応領域における画像を平面パラメータに応じて射影変換してなる画像の画素値分布との間の誤差を表す誤差関数を線形結合して構成される評価関数を算出し、この評価関数の値を最小化するように、段差エッジの位置及び方向を表す段差エッジパラメータを探索的に特定する。
【解決手段】撮像手段３Ｒ，３Ｌにより撮像した２つの撮像画像（ステレオ画像）を用いて段差部（階段）５０の段差エッジ５２の位置及び方向を推定する。第１の撮像画像に設定した領域における画像の素値分布と、第２の撮像画像の対応領域における画像を平面パラメータに応じて射影変換してなる画像の画素値分布との間の誤差を表す誤差関数を線形結合して構成される評価関数を算出し、この評価関数の値を最小化するように、段差エッジの位置及び方向を表す段差エッジパラメータを探索的に特定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、撮像画像を用いて階段のエッジ等の段差エッジの位置及び方向を推定する装置に関する。
【背景技術】
【０００２】
従来、例えば自律的に移動可能なロボットに階段の昇降動作を行なわせる場合に、該ロボットに階段を認識させる技術として、例えば特許文献１に見られる技術が提案されている。
【０００３】
この技術では、ロボットに搭載したステレオビジョンシステムによってロボットの外界の距離データを取得し、この距離データを基に、階段の踏面に相当する平面部を検出する。そして、検出した平面部を包含する多角形をSklanskyのアルゴリズムや、Melkmanのアルゴリズムによって求め、その求めた多角形から、階段のフロントエッジ、バックエッジ、幅、長さを特定する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】国際公開公報ＷＯ2005/087452
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ところで、２足移動ロボット等の脚式移動ロボットに階段等の段差部の昇降動作を行なわせる場合、ロボットの脚の足平部を、段差部の上段側又は下段側の平坦面部（以下、踏面ということがある）の先端部のエッジ（特許文献１で言うところのフロントエッジ）に載せて、該足平部の一部を段差部の踏面からはみ出させることが必要もしくは好ましい場合がある。
【０００６】
例えば、階段の踏面の奥行き寸法が、ロボットの足平部の大きさに比して狭い場合には、該足平部の全体を踏面に載せることが不可能な場合がある。あるいは、ロボットの足平部の全体を踏面に載せることが可能であっても、該足平部を振り出すときに、該足平部が階段の蹴り込み等と干渉するのを避けるために、ロボットの足平部の一部を踏面からはみ出させることが好ましい場合もある。
【０００７】
そして、このような場合には、ロボットの足平部の目標とする着地位置を適切に決定するために、階段等の段差部の先端部のエッジの位置及びその方向を精度よく認識する必要がある。
【０００８】
しかるに、前記特許文献１に見られる如き従来の手法では、階段などの段差部の踏面の先端部のエッジを検出することができない場合が多々ある。
【０００９】
すなわち、ステレオビジョンシステムによって得られるステレオ画像から認識される距離データは、撮像画像のテクスチャ等の影響を受けやすいので、段差部の踏面の先端部のエッジ付近での信頼性の高い距離データを得ることができない場合も多々ある。そして、このような場合には、段差部の踏面の先端部のエッジ付近は、平面部として認識できないか、もしくは、平面部から除外されることとなる。その結果、前記多角形は、段差部の踏面の先端部のエッジと異なる境界の形状を有するものとなり、該多角形から認識されるフロントエッジは、段差部の踏面の先端部の実際のエッジとの誤差が大きなものとなりやすい。
【００１０】
従って、前記特許文献１に見られる如き従来の手法では、階段などの段差部の踏面の先端部のエッジを検出することができない場合が多々ある。
【００１１】
本発明は、かかる背景に鑑みてなされたものであり、撮像手段により取得されるステレオ画像を利用して、階段等の段差部の平坦面部の先端部のエッジの位置及び方向を精度良く推定することを可能とする装置を提供することを目的とする。
【課題を解決するための手段】
【００１２】
ここで、本発明における用語について補足しておく。本発明においては、「段差部」は、上段側の平坦面部とその一段下側に存在する下段側の平坦面部との組が一組もしくは複数組含まれるものである。この「段差部」は、階段を含むものであるが、階段だけを意味するものではない。例えば、床面に設置される踏み台もしくは任意の設置物により構成される段差部も含まれる。
【００１３】
また、段差部の平坦面部の「先端側エッジ」又は「段差エッジ」というのは、段差部をその下段側が手前側、上段側が奥側となる視線で見た場合において、各段の平坦面部（階段で言うところの踏面）の外形の境界線のうち、手前側で横方向（水平方向もしくはそれに近い方向）に直線状に延在する境界線を意味する。
【００１４】
あるいは、換言すれば、段差部の平坦面部の「先端側エッジ」又は「段差エッジ」というのは、段差部をその上段側が手前側、下段側が奥側となる視線で見た場合において、各段の平坦面部の外形の境界線のうち、奥側で横方向に直線状に延在する境界線を意味する。
【００１５】
以上を前提として、本発明を以下に説明する。
【００１６】
本発明は、上記目的を達成するために、第１の撮像画像及び第２の撮像画像から構成されるステレオ画像を撮像するための撮像手段を用い、下段側の平坦面部と、該下段側の平坦面部の一段上側の上段側の平坦面部との組を少なくとも一組有する段差部を、その下段側が手前側、上段側が奥側となる視線で前記撮像手段により撮像し、その撮像により得られた該段差部のステレオ画像を基に、前記上段側の平坦面部の先端側エッジである段差エッジの３次元空間中の位置及び方向を推定する段差エッジ推定装置であって、
前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値を、暫定的に仮決定した段差エッジパラメータと、前記上段側の平坦面部を包含する上段側平面の位置及び姿勢を表すものとして事前に特定された上段側平面パラメータと、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジを含むと共に前記上段側平面に対して起立した姿勢となる起立平面の位置及び姿勢を表すものとして、前記仮決定した段差エッジパラメータ及び前記上段側平面パラメータに応じて設定した起立平面パラメータとを用いて算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する段差エッジ特定手段を備え、
前記評価関数は、
前記第１の撮像画像中に設定した第１の領域における画像の画素値分布と、前記第２の撮像画像中の前記第１の領域に対応する領域における画像を前記起立平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第１誤差関数と、
前記第１の撮像画像中に設定した第２の領域における画像の画素値分布と、前記第２の撮像画像中の前記第２の領域に対応する領域における画像を前記上段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第２誤差関数とを線形結合して構成された評価関数であり、
前記段差エッジ特定手段は、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第１の撮像画像中において前記第１の領域及び第２の領域の境界線となると共に、前記第１の領域が、前記第１の撮像画像中において前記上段側の平坦面部とその一段下側の下段側の平坦面部との間の部分である上下段間部分と前記上段側の平坦面部とのうちの上下段間部分寄りの領域、前記第２の領域が前記上下段間部分と前記上段側の平坦面部とのうちの上段側の平坦面部寄りの領域となるように該第１の領域及び第２の領域を設定する評価関数算出用領域設定手段を含むことを特徴とする（第１発明）。
【００１７】
ここで、一般に、前記段差部の上段側の平坦面部の先端部には、階段における段鼻の先端面の如く、上段側の平坦面部からこれに対して起立した姿勢で垂下するほぼ平坦な起立面が存在し、この起立面を包含する起立平面と上段側の平坦面部を包含する上段側平面との交線が前記段差エッジに相当するものである。そして、前記段差部をその下段側が手前側、上段側が奥側となる視線で前記撮像手段により撮像した場合には、該起立面が第１の撮像画像及び第２の撮像画像に投影されることとなる。
【００１８】
また、各撮像画像における上段側の平坦面部や下段側の平坦面部は比較的広い面積を有することから、それらの平坦面部の撮像手段に対する位置及び姿勢は、距離センサや前記ステレオ画像等を利用した公知の手法によって、比較的容易に特定しておくことができる。
【００１９】
さらに、前記段差エッジの位置及び方向を、暫定的に、ある適当な位置及び方向に仮決定し、その仮決定した位置及び方向を有する段差エッジが上段側平面との交線となるように起立平面を設定した場合に、その起立平面が実際の段差部における前記起立面を包含する平面であるか否かは、前記第１の撮像画像及び第２の撮像画像から構成されるステレオ画像と射影変換（平面射影変換）とを利用して評価することが可能である。
【００２０】
前記第１発明は、これらの事項を考慮して構築されたものである。
【００２１】
この第１発明によれば、前記段差エッジ特定手段は、前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値の大きさを最小化するように、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する。
【００２２】
この場合、上記評価関数は、前記第１誤差関数と第２誤差関数とを線形結合して構成される。そして、この第１誤差関数は、前記第１の撮像画像中に設定した第１の領域における画像の画素値分布と、前記第２の撮像画像中の前記第１の領域に対応する領域における画像を、暫定的に仮決定した段差エッジパラメータと事前に特定された上段側平面パラメータとに応じて設定した前記起立平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す関数である。
【００２３】
また、第２誤差関数は、前記第１の撮像画像中に設定した第２の領域における画像の画素値分布と、前記第２の撮像画像中の前記第２の領域に対応する領域における画像を前記上段側平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す関数である。
【００２４】
さらに、前記第１の領域と第２の領域とは、前記した如く、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジを前記第１の撮像画像中における両領域の境界線として設定される領域である。
【００２５】
このため、前記評価関数の値は、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジの位置及び方向の、実際の段差エッジの位置及び方向に対する誤差の度合いを表すものとなり、暫定的に仮決定した段差エッジパラメータにより規定される段差エッジの位置及び方向が、実際の段差エッジの位置及び方向に合致していれば、前記評価関数の値は、最小となるものである。
【００２６】
従って、暫定的に仮決定した段差エッジパラメータに対して前記評価関数を算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、前記最適段差エッジパラメータを特定することによって、該最適段差エッジパラメータを、実際の段差エッジの位置及び姿勢の推定値を表すものとして得られることとなる。
【００２７】
なお、前記評価関数の値の大きさを最小化するように、前記最適段差エッジパラメータを特定するということは、評価関数の値が最小値、もしくは、その近似値（当該最小値との差が所要の許容範囲（段差エッジの位置及び方向の推定精度を目標とする精度に確保するために要求される許容範囲）内に収まる値）となると見なし得る段差エッジパラメータを、最適段差エッジパラメータとして特定することを意味する。従って、特定した最適段差エッジパラメータに対して算出される前記評価関数の値が厳密に該評価関数の最小値である必要はない。
【００２８】
かかる第１発明によれば、前記段差エッジが、前記上段側平面と起立平面との交線となることを前提とすることによって、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジの位置及び方向の、実際の段差エッジの位置及び方向に対する誤差の度合いを、前記した如く構成した評価関数の値によって適正に評価することができる。このため、撮像手段により取得される第１の撮像画像及び第２の撮像画像から構成されるステレオ画像を利用して、階段等の段差部の平坦面部の先端部のエッジの位置及び方向を精度良く推定することが可能となる。
【００２９】
なお、第１誤差関数の代表的な例としては、前記第１の撮像画像中の第１の領域における画像の各画素値（輝度値など）と、前記第２の撮像画像中の前記第１の領域に対応する領域における画像の射影変換画像の各画素値との偏差の絶対値又は二乗値の積算値、あるいは、該絶対値又は二乗値の平均値が挙げられる。第２誤差関数についても同様である。
【００３０】
前記第１発明では、前記段差エッジ特定手段は、前記上段側平面又は水平面に対する前記起立平面の傾斜角度を、事前に設定された所定の角度に規定して前記起立平面パラメータを設定することが好ましい（第２発明）。
【００３１】
この第２発明によれば、前記上段側平面又は水平面に対する前記起立平面の傾斜角度が既定値とされることとなるので、暫定的に仮決定した段差エッジパラメータと、前記上段側平面パラメータとから前記起立平面パラメータを設定できる。このため、未知数が、段差エッジパラメータだけとなり、前記最適段差エッジパラメータの探索的な特定処理が容易になる。
【００３２】
但し、第１発明においては、前記起立平面の傾斜角度と、段差エッジパラメータとの組を未知数として、該傾斜角度と最適段差エッジパラメータとを特定するようにすることも可能である。
【００３３】
また、第１発明又は第２発明においては、前記評価関数算出用領域設定手段は、前記第１の撮像画像中において、前記第１の領域の前記第２の領域と反対側の境界線が、前記暫定的な段差エッジと所定の間隔を存して平行に延在するように該第１の領域を設定する手段を含んでおり、該所定の間隔は、前記暫定的な段差エッジが実際の段差エッジに一致する場合に、前記第１の領域の前記第２の領域と反対側の境界線が、前記上下段間部分において前記上段側の平坦面部とその一段下側の下段側の平坦面部とのうちの上段側の平坦面部寄りの境界線となるように設定された間隔であることが好ましい（第３発明）。
【００３４】
すなわち、一般的な階段等の段差部において、上段側の平坦面部の先端部で、該平坦面部からこれに対して起立した姿勢で垂下する起立面は、多くの場合、下段側の平坦面部まで同一平面上で連なっているわけではなく、上段側の平坦面部寄りの箇所で、高さ方向に比較的小さな幅（上段側の平坦面部を構成する板部材の厚さ分に相当する幅）を有する面となっている場合が多い。そして、上段側の平坦面部と下段側の平坦面部との間の箇所（階段の蹴込みに相当する箇所）のうちの、下段側の平坦面部寄りの箇所には、起立面が存在しないか、又は、上段側の平坦面部に連なる起立面よりも奥側に存在する面となっている場合が多い。
【００３５】
そこで、第３発明では、上記の如く、第１の領域を設定するようにした。これにより、特定の種類に限られない一般的な形状の種々様々な段差部に対して、前記暫定的な段差エッジが実際の段差エッジに一致する場合における第１の領域の全体又は大部分が、段差部における前記起立平面上に存在する部分の画像となるように、前記第１の領域を設定できることとなる。この結果、一般的な形状の種々様々な段差部に対して、前記評価関数の信頼性（暫定的な段差エッジの位置及び方向の誤差を表すものとしての信頼性）を高め、ひいては、実際の段差エッジの位置及び方向の推定の信頼性を高めることができると共に、その推定処理の汎用性を高めることができる。
【００３６】
前記第１〜第３の発明において、前記最適段差エッジパラメータを特定するための代表的な手法としては、次のような２つの手法を用いることができる。その第１の手法では、前記段差エッジ特定手段は、暫定的に仮決定する前記段差エッジパラメータの候補としてあらかじめ定められた複数の段差エッジパラメータ候補のうち、当該候補に対応して算出される前記評価関数の値が最小となる段差エッジパラメータ候補を前記最適段差エッジパラメータとして特定するように構成される。
【００３７】
また、第２の手法では、前記段差エッジ特定手段は、暫定的に仮決定する前記段差エッジパラメータを、その初期値から逐次更新していくことによって、前記最適段差エッジパラメータを特定するように構成される。なお、暫定的に仮決定する前記段差エッジパラメータを、その初期値から逐次更新していくということは、より詳しくは、段差エッジパタメータを暫定的に仮決定し、少なくともその仮決定した段差エッジパラメータに対応して算出される評価関数の値に応じて、新たな暫定的な段差エッジパラメータを仮決定するという処理を繰り返すことを意味する。
【００３８】
前記第１〜第３の発明においては、上記第１の手法及び第２の手法のいずれの手法を採用することもできる。そして、特に上記第２の手法を採用する場合には、前記初期値は、次のように設定することが好適である。すなわち、前記第１〜第３の発明において、前記段差エッジ特定手段が、暫定的に仮決定する前記段差エッジパラメータを、その初期値から逐次更新していくことによって、前記最適段差エッジパラメータを特定するように構成される場合には、該段差エッジ特定手段は、前記段差エッジパラメータの初期値により規定される暫定的な段差エッジが、前記上段側の平坦面部のうち、平面であることが事前に特定されている領域を通るように該初期値を設定する手段を含むことが好ましい（第４発明）。
【００３９】
この第４発明によれば、暫定的に仮決定する前記段差エッジパラメータの初期値を、上記の如く設定することによって、前記最適段差エッジパラメータを特定するための暫定的な段差エッジパラメータの更新処理の安定性（ロバスト性）を高め、暫定的な段差エッジパラメータが発散してしまうように更新されてしまうのを防止することができる。
【００４０】
この第４発明では、前記撮像手段が、前記段差部を移動する移動体に、該移動体の進行方向前方を撮像するように搭載されている場合には、前記段差エッジパラメータの初期値を設定する手段は、該初期値により規定される暫定的な段差エッジの方向が前記移動体の進行方向に対して交差する方向になるように該初期値を設定することが好ましい（第５発明）。
【００４１】
この第５発明よれば、暫定的に仮設定する段差エッジパラメータの初期値により規定される段差エッジの方向が、実際の方向に近い方向となるので、段差エッジパラメータの更新処理の安定性（ロバスト性）を効果的に高めることができる。
【００４２】
また、前記第１〜第５発明では、前記評価関数は、前記第１の撮像画像中に設定した第３の領域における画像の画素値分布と、前記第２の撮像画像中の前記第３の領域に対応する領域における画像を、前記上段側の平坦面部の一段下側の下段側の平坦面部を包含する下段側平面の位置及び姿勢を表すものとして事前に特定された下段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第３誤差関数を、前記第１誤差関数と第２誤差関数とにさらに線形結合して構成された評価関数であり、
前記評価関数算出用領域設定手段は、前記第３の領域が、前記第１の撮像画像中において前記第１の領域よりも前記下段側の平坦面部寄りの位置で該下段側の平坦面部の少なくとも一部分を含むと共に、該第３の領域の前記第１の領域側の境界線の該第１の撮像画像中での位置及び方向を、該第１の撮像画像中での前記暫定的な段差エッジの位置及び方向に応じて規定するように該第３の領域を設定する手段を含むようにしてもよい（第６発明）。
【００４３】
この第６発明によれば、前記評価関数は、前記第１誤差関数及び第２誤差関数に、さらに、前記第３誤差関数を加えた３つの誤差関数を線形結合することによって構成される。そして、この場合、前記第３誤差関数を算出するための第１の撮像画像中の第３の領域は、上記の如く設定される。
【００４４】
このため、第１及び第２の撮像画像における段差部の上段側の平坦面部及び段差エッジの近辺の撮像画像に加えて、下段側の平坦面部の近辺の撮像画像の画素値データが、暫定的に仮決定する段差エッジパラメータに依存性を有するように前記第３誤差関数を介して評価関数に反映されることとなる。この結果、この評価関数の値を最小化するように決定した最適段差エッジパラメータの信頼性と、該最適段差エッジパラメータの探索的な特定処理のロバスト性とを高めることができる。
【００４５】
この第６発明では、前記第３の領域を設定する手段は、該第３の領域の前記第１の領域側の境界線が、前記暫定的な段差エッジと平行に延在するように該第３の領域を設定することが好ましい（第７発明）。
【００４６】
この第７発明によれば、暫定的に仮決定する段差エッジパラメータを前記第３の領域に適切に反映させ、前記第３の誤差関数が、暫定的に仮決定する段差エッジパラメータに依存して変化するようにすることができる。ひいては、前記評価関数に、下段側の平坦面部の近辺の撮像画像の画素値データを適切に反映させることができる。
【００４７】
上記第６発明又は第７発明では、前記第３の領域を設定する手段は、前記第１の撮像画像において第１の領域と第３の領域との間に所定の間隔を存するように該第３の領域を設定するものであり、該所定の間隔は、前記暫定的な段差エッジが実際の段差エッジに一致する場合に、前記第３の領域の前記第１の領域側の境界線が、前記第１の撮像画像において前記下段側の平坦面部内に存在するように設定された間隔であることが好ましい（第８発明）。
【００４８】
これによれば、前記第１の領域と第３の領域との間に、上記所定の間隔を設定しておくことで、第１の領域と第３の領域との間の間隔内の画像は前記第３誤差関数、ひいては、前記評価関数の値に影響を及ぼさないものとなる。そして、この場合、上記所定の間隔は、前記暫定的な段差エッジが実際の段差エッジに一致する場合に、前記第３の領域の前記第１の領域側の境界線が、前記第１の撮像画像において前記下段側の平坦面部内に存在するように設定された間隔、すなわち、前記暫定的な段差エッジが実際の段差エッジに一致する場合に、第３領域内に前記上下段間部分の画像が含まれないように設定された間隔である。
【００４９】
このため、種々様々の形状の段差部に対して、上段側の平坦面部の先端部の前記起立面と下段側の平坦面部とのいずれとも異なる部分の画像が、前記第１誤差関数又は第３誤差関数の値、ひいては、前記評価関数の値に影響を及ぼすのを極力抑制することができる。その結果、一般的な形状の種々様々な段差部に対して、前記評価関数の信頼性（暫定的な段差エッジの位置及び方向の誤差を表すものとしての信頼性）を高め、ひいては、実際の段差エッジの位置及び方向の推定の信頼性を高めることができると共に、その推定処理の汎用性を高めることができる。
【００５０】
また、前記第６発明では、前記第３の領域を設定する手段は、次のような形態で前記第３の領域の前記第１の領域側の境界線を設定してもよい。
【００５１】
すなわち、前記第６発明において、前記第３の領域を設定する手段は、前記第３の領域の前記第１の領域側の境界線が、前記仮決定した段差エッジパラメータ及び前記上段側平面パラメータに応じて設定された前記起立平面パラメータに対応する前記起立平面と前記下段側平面との交線を前記第１の撮像画像に投影した直線上の線分になるように、該第３の領域の第１の領域側の境界線を設定するようにしてもよい（第９発明）。
【００５２】
この第９発明によれば、暫定的に仮決定する段差エッジパラメータを前記第３の領域に適切に反映させ、前記第３の誤差関数が、暫定的に仮決定する段差エッジパラメータに依存して変化するようにすることができる。ひいては、第６発明と同様に、前記評価関数に、下段側の平坦面部の近辺の撮像画像の画素値データを適切に反映させることができる。
【００５３】
さらに、種々様々の形状の段差部に対して、上段側の平坦面部の先端部の前記起立面と下段側の平坦面部とのいずれとも異なる部分の画像が、前記第１誤差関数又は第３誤差関数の値、ひいては、前記評価関数の値に影響を及ぼすのを極力抑制することができる。その結果、第７発明と同様に、一般的な形状の種々様々な段差部に対して、前記評価関数の信頼性（暫定的な段差エッジの位置及び方向の誤差を表すものとしての信頼性）を高め、ひいては、実際の段差エッジの位置及び方向の推定の信頼性を高めることができると共に、その推定処理の汎用性を高めることができる。
【００５４】
前記段差部をその下段側が手前側、上段側が奥側となる視線で前記撮像手段により撮像した場合には、前記第１〜第９発明によって段差エッジの位置及び方向を推定することができる。一方、前記段差部をその上段側が手前側、下段側が奥側となる視線で前記撮像手段により撮像した場合には、階段の段鼻の先端面の如き段差部の起立面は、前記第１の撮像画像及び第２の撮像画像には写らない。この場合には、以下に説明する第１０発明によって、段差エッジの位置及び方向を推定することができる。
【００５５】
すなわち、第１０発明の段差エッジ推定装置は、第１の撮像画像及び第２の撮像画像から構成されるステレオ画像を撮像するための撮像手段を用い、下段側の平坦面部と、該下段側の平坦面部の一段上側の上段側の平坦面部との組を少なくとも一組有する段差部を、その上段側が手前側、下段側を奥側となる視線で前記撮像手段により撮像し、その撮像により得られた該段差部のステレオ画像を基に、前記上段側の平坦面部の先端側エッジである段差エッジの３次元空間中の位置及び方向を推定する段差エッジ推定装置であって、
前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値を、暫定的に仮決定した段差エッジパラメータと、前記上段側の平坦面部を包含する上段側平面の位置及び姿勢を表すものとして事前に特定された上段側平面パラメータと、前記上段側の平坦面部の一段下側の下段側平坦面部を包含する下段側平面の位置及び姿勢を表すものとして事前に特定された下段側平面パラメータとを用いて算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する段差エッジ特定手段を備え、
前記評価関数は、
前記第１の撮像画像中に設定した第４の領域における画像の画素値分布と、前記第２の撮像画像中の前記第４の領域に対応する領域における画像を前記上段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第４誤差関数と、
前記第１の撮像画像中に設定した第５の領域における画像の画素値分布と、前記第２の撮像画像中の前記第５の領域に対応する領域における画像を前記下段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第５誤差関数とを線形結合して構成された評価関数であり、
前記段差エッジ特定手段は、前記第１の撮像画像中において、前記第４の領域が、前記上段側の平坦面部と前記下段側の平坦面部とのうちの上段側の平坦面部寄りの領域、前記第５の領域が前記上段側の平坦面部と前記下段側の平坦面部とのうちの下段側の平坦面部寄りの領域となると共に、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第１の撮像画像中において前記第４の領域の前記第５の領域側の境界線となり、且つ、前記第５の領域の前記第４の領域側の境界線が、前記第４の領域の前記第５の領域側の境界線に一致するか、又は、前記第４の領域の前記第５の領域側の境界線と間隔を存して当該第５の領域側の境界線と平行に延在するように該第４の領域及び第５の領域を設定する評価関数算出用領域設定手段を含むことを特徴とする（第９発明）。
【００５６】
ここで、前記段差部をその上段側が手前側、下段側が奥側となる視線で前記撮像手段により撮像した場合には、前記第１発明に関して説明した上段側平坦面部の先端部の起立面が第１の撮像画像及び第２の撮像画像に投影されず、それらの第１の撮像画像及び第２の撮像画像中では、段差エッジは、上段側の平坦面部と下段側の平坦面部との境界線として写ることとなる。上記第１０発明は、このことを考慮して構築されたものである。
【００５７】
この第１０発明によれば、前記段差エッジ特定手段は、前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値の大きさを最小化するように、前記暫定的に仮決定した段差エッジパラメータを修正することによって、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する。
【００５８】
この場合、第１０発明における上記評価関数は、前記第４誤差関数と第５誤差関数とを線形結合して構成される。そして、この第４誤差関数は、前記第１の撮像画像中に設定した第４の領域における画像の画素値分布と、前記第２の撮像画像中の前記第４の領域に対応する領域における画像を、前記上段側平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す関数である。
【００５９】
また、第５誤差関数は、前記第１の撮像画像中に設定した第５の領域における画像の画素値分布と、前記第２の撮像画像中の前記第５の領域に対応する領域における画像を、前記下段側平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す関数である。
【００６０】
さらに、前記第４の領域と第５の領域とは、前記した如く、前記第１の撮像画像中において、前記第４の領域が、前記上段側の平坦面部と前記下段側の平坦面部とのうちの上段側の平坦面部寄りの領域、前記第５の領域が前記上段側の平坦面部と前記下段側の平坦面部とのうちの下段側の平坦面部寄りの領域となると共に、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第１の撮像画像中において前記第４の領域の前記第５の領域側の境界線となり、且つ、前記第５の領域の前記第４の領域側の境界線が、前記第４の領域の前記第５の領域側の境界線に一致するか、又は、前記第４の領域の前記第５の領域側の境界線と間隔を存して当該第５の領域側の境界線と平行に延在するように設定される領域である。
【００６１】
このため、第１０発明における前記評価関数の値は、第１発明における評価関数と同様に、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジの位置及び方向の、実際の段差エッジの位置及び方向に対する誤差の度合いを表すものとなり、暫定的に仮決定した段差エッジパラメータにより規定される段差エッジの位置及び方向が、実際の段差エッジの位置及び方向に合致していれば、前記評価関数の値は、最小となるものである。
【００６２】
従って、暫定的に仮決定した段差エッジパラメータに対して前記評価関数を算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、前記暫定的に仮決定した段差エッジパラメータを修正することによって、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定できることとなる。
【００６３】
よって、第１０発明によれば、前記段差エッジが、第１及び第２の撮像画像中において、上段側の平坦面部と下段側の平坦面部との境界線として写ることを前提とすることによって、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジの位置及び方向の、実際の段差エッジの位置及び方向に対する誤差の度合いを、前記した如く構成した評価関数の値によって適正に評価することができる。このため、撮像手段により取得される第１の撮像画像及び第２の撮像画像から構成されるステレオ画像を利用して、階段等の段差部の平坦面部の先端部のエッジの位置及び方向を精度良く推定することが可能となる。
【００６４】
なお、第４誤差関数の代表的な例としては、前記第１の撮像画像中の第４の領域における画像の各画素値（輝度値など）と、前記第２の撮像画像中の前記第４の領域に対応する領域における画像の射影変換画像の各画素値との偏差の絶対値又は二乗値の積算値、あるいは、該絶対値又は二乗値の平均値が挙げられる。第５誤差関数についても同様である。
【００６５】
また、第１０発明において、前記最適段差エッジパラメータを特定するための代表的な手法としては、前記第１〜第３発明に関して説明した第１の手法又は第２の手法と同じ手法を用いることができる。
【００６６】
上記第１０発明において、所謂、オクルージョンによって、第１の撮像画像に写っている下段側の平坦面部のうちの一部、例えば、下段側の平坦面部のうちの、上段側の平坦面部の段差エッジに近い部分が第２の撮像画像には写っていないという場合がある。
【００６７】
そして、このようなオクルージョンが発生している場合には、前記第１の撮像画像において前記第５の領域の前記第４の領域側の境界線が、前記第４の領域の前記第５の領域側の境界線に一致するようにした場合（前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第１の撮像画像中において前記第４の領域と第５の領域との間の境界線となる場合）には、暫定的な段差エッジが実際の段差エッジに一致していても、前記第５誤差関数に上記オクルージョンに起因する誤差成分が含まれることとなる。
【００６８】
従って、上記の如きオクルージョンが発生する状況では、前記第５の領域の前記第４の領域側の境界線が、前記第４の領域の前記第５の領域側の境界線と間隔を存して当該第５の領域側の境界線と平行に延在するように第５の領域を設定して、第５の領域に、第２の撮像画像中に写らないような部分が含まれないようにすることが好ましい。
【００６９】
そこで、第１０発明では、前記評価関数算出用領域設定手段は、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジを前記第１の撮像画像に投影してなる線である第１暫定段差エッジ線を、前記上段側平面パラメータに応じて第１の撮像画像側から第２の撮像画像側に射影変換した場合に得られる第２Ａ暫定段差エッジ線と、前記第１暫定段差エッジ線を、前記下段側平面パラメータに応じて第１の撮像画像側から第２の撮像画像側に射影変換した場合に得られる第２Ｂ暫定段差エッジ線との第２の撮像画像中での位置関係を比較し、該２の撮像画像での第２Ａ暫定段差エッジ線の位置が、前記第２Ｂ暫定段差エッジ線よりも、前記上段側の平坦面部から下段側の平坦面部に向う向きに変位した位置であるか否かを判断する手段と、該判断結果が肯定的である場合には、前記第５の領域の前記第４の領域側の境界線が、前記第２Ａ暫定段差エッジ線を、前記下段側平面パラメータに応じて第２の撮像画像側から第１の撮像画像側に射影変換した場合に得られる線上に存在するように前記第５の領域を設定し、前記判断結果が否定的である場合には、前記第５の領域の前記第４の領域側の境界線が、前記第１暫定段差エッジ線上に存在するように前記第５の領域を設定する手段とを備えることが好ましい（第１１発明）。
【００７０】
この第１１発明によれば、第１の撮像画像に写っている下段側の平坦面部のうち、上段側の平坦面部の段差エッジに近い部分が第２の撮像画像には写っていないというようなオクルージョンが発生していても、前記暫定的な段差エッジが、実際の段差エッジに一致もしくはほぼ一致するような状況で、上記オクルージョンに起因する誤差成分が前記第５誤差関数に極力含まれないようにすることができる。
【００７１】
ひいては、前記暫定的な段差エッジと実際の段差エッジとの一致度合いを反映するものとしての前記誤差関数の値の信頼性を高めることができる。その結果、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータの信頼性をより一層高めることができる。
【図面の簡単な説明】
【００７２】
【図１】図１（ａ），（ｂ）はそれぞれ、本発明の一実施形態におけるロボットが階段を昇ろうとしている状況、降ろうとしている状況を示す斜視図。
【図２】代表的な階段の構造を説明するための図。
【図３】図１に示すロボットに備えた演算処理装置の機能的構成を示すブロック図。
【図４】図４（ａ），（ｂ）はそれぞれ、実施形態で説明する段差部（階段）に関する平面と平面パラメータベクトルとを例示する斜視図、側面視図。
【図５】図５（ａ），（ｂ）は、それぞれ第１の撮像画像を例示する図。
【図６】図６（ａ），（ｂ）は、それぞれ第１の撮像画像を例示する図。
【図７】図３に示した段差エッジ特定手段の処理（図１（ａ）に示す状況での処理）を示すブロック図。
【図８】図７のＳ４，Ｓ５，Ｓ６の処理を説明するための図。
【図９】図７のＳ４，Ｓ５，Ｓ６の処理の他の例を説明するための図。
【図１０】図７のＳ１における暫定段差エッジパラメータの更新処理を説明するための図。
【図１１】図３に示した段差エッジ特定手段の処理（図１（ｂ）に示す状況での処理）を示すブロック図。
【図１２】図７のＳ３における領域Ａb1の設定手法の他の例を説明するための図。
【図１３】図１１のＳ２２における領域Ａa3，Ａb3の設定手法の他の例を説明するための図。
【発明を実施するための形態】
【００７３】
本発明の一実施形態を図１〜図１１を参照して以下に説明する。
【００７４】
図１（ａ），（ｂ）を参照して、本実施形態では、移動体としての脚式移動ロボット１によって階段等の段差部の段差エッジの位置及び方向を推定する場合を例にとって説明する。図示例の脚式移動ロボット１（以下、単にロボット１という）は、左右一対の２つの脚体を備える２足歩行ロボットである。
【００７５】
このロボット１には、２つの撮像画像から構成されるステレオ画像を撮像する撮像手段として、左右一対のカメラ３Ｒ，３Ｌが搭載され、これらのカメラ３Ｒ，３Ｌによって、ロボット１の周辺環境（主に、ロボット１の進行方向前方の環境）のステレオ画像を取得することが可能となっている。なお、カメラ３Ｒ，３Ｌの撮像画像は、モノトーン画像及びカラー画像のいずれの画像であってもよい。
【００７６】
そして、ロボット１は、これらのカメラ３Ｒ，３Ｌによって取得したステレオ画像等を基に、周辺環境の床形状や設置物等を認識しつつ移動する。
【００７７】
例えば、図１（ａ）に示すように、ロボット１がその進行方向前方に存在する段差部としての階段５０を昇ろうとしている状況、あるいは、図１（ｂ）に示すように、ロボット１がその進行方向前方に存在する段差部としての階段５０を降ろうとしている状況において、ロボット１は、カメラ３Ｒ，３Ｌによって取得した進行方向前方の環境のステレオ画像を基に、階段５０の各段の平坦面部５１（詳しくは、ほぼ水平な平坦面として認識し得る部分）の３次元空間中の位置及び姿勢（法線方向）や、各段の平坦面部５１の先端側エッジとしての段差エッジ５２の３次元空間中の位置及び方向を認識する。
【００７８】
ここで、ロボット１が認識する階段５０の各段の平坦面部５１の段差エッジ５２は、図１（ａ）に示すロボット１の如く、階段５０をその下段側が手前側、上段側が奥側となる視線で見た場合においては、上段側の平坦面部５１の外形の境界線のうち、手前側で横方向（階段５０の幅方向）に直線状に延在する境界線である。
【００７９】
また、図１（ｂ）に示すロボット１の如く、階段５０をその上段側が手前側、下段側が奥側となる視線で見た場合にいては、段差エッジ５２は、上段側の平坦面部５１の外形の境界線のうち、奥側で横方向（階段５０の幅方向）に直線状に延在する境界線である。
【００８０】
補足すると、図２を参照して、階段の各段の平坦面部は、一般に踏面と言われる部分である。そして、この踏面の先端部は、一般に段鼻と言われる部分である。従って、上記階段５０の各段の平坦面部５１は、換言すれば、階段５０の各段の踏面を意味し、また、上記段差エッジ５２は、換言すれば、階段５０の各段の段鼻の上面の先端側境界線を意味する。
【００８１】
なお、階段の上段側の踏面（平坦面部）の先端側と、その一段下側の下段側の踏面（平坦面部）の奥側との間での部分は、一般に蹴込みと言われる。そして、階段の各段の踏面（平坦面部）の先端部（段鼻）は、多くの場合、図２に示す如く、その先端面が、上記蹴込みの箇所に起立した姿勢で形成される面部（以降、蹴込み面部という）よりも若干突き出るように形成される。
【００８２】
但し、本実施形態で、段差エッジの推定対象として主に説明する階段では、段鼻の先端面が、蹴込み面部よりも若干突き出ている必要はない。例えば、図１（ａ），（ｂ）に簡略的に記載した階段５０のように、段鼻の先端面と蹴込み面部とが面一もしくはほぼ面一に形成されていてもよい。また、本実施形態で、段差エッジの推定対象として主に説明する階段は、蹴込みの箇所に、蹴込み面部を形成する板部材が設けられていない（蹴込み面部が存在しない）構造のものであってもよい。
【００８３】
図３を参照して、ロボット１には、ＣＰＵ、ＲＡＭ、ＲＯＭを含む電子回路ユニットにより構成された演算処理装置１０が搭載されており、カメラ３Ｒ，３Ｌによりそれぞれ撮像された撮像画像の画素値データ（撮像画像を構成する画素毎の輝度、彩度、色相等を表すデータ）が演算処理装置１０に入力されて、図示しない画像メモリに逐次更新されつつ記憶保持されるようになっている。
【００８４】
演算処理装置１０は、実装されたプログラムによって実現される機能として、ロボット１の進行方向前方に存在する平坦面部の位置及び姿勢（法線方向）を認識する平坦面部認識手段１１と、階段等の段差部の段差エッジの位置及び方向を表す段差エッジパラメータを探索的な手法によって特定する段差エッジ特定手段１２とを備えており、これらの手段１１，１２の処理によって、本発明の段差エッジ推定装置としての機能を実現する。なお、演算処理装置１０は、段差エッジ推定装置としての機能の他に、ロボット１の動作制御を行なう機能を備えていてもよい。
【００８５】
上記平坦面部認識手段１１及び段差エッジ特定手段１２のより詳細な処理を以下に説明する。
【００８６】
カメラ３Ｒ，３Ｌによって撮像されたロボット１の進行方向前方の環境のステレオ画像を構成する２つの撮像画像の画素値データが平坦面部認識手段１１に与えられる。そして、平坦面部認識手段１１は、与えられた２つの撮像画像の画素値データから、公知のステレオマッチングの手法によって、両撮像画像に捉えられている物体の各部の空間的な位置を算出する。その空間的な位置は、カメラ３Ｒ，３Ｌに対して固定されたカメラ座標系での位置である。該カメラ座標系としては、本実施形態では、図４（ａ），（ｂ）に例示する如く、カメラ３Ｒ，３Ｌのいずれか一方の光学中心、例えばカメラ３Ｒの光学中心Ｃ0に原点を設定した３軸直交座標系（ｘｙｚ座標系）Ｃcamが用いられる。
【００８７】
さらに、平坦面部認識手段１１は、算出した位置データを基に、各撮像画像（あるいはいずれか一方の撮像画像）において、空間的な高さがほぼ一定となる領域（該領域内の各部に対応する空間的な高さが互いにほぼ同一となる領域）、すなわち、ほぼ水平な面と見なせる領域が投影された画像部分を平坦面部投影領域として抽出する。
【００８８】
例えば、図１（ａ）又は（ｂ）に示した状況で、階段５０をカメラ３Ｒ，３Ｌにより撮像し、カメラ３Ｒ，３Ｌのいずれか一方、例えばロボット１の右側のカメラ３Ｒによって、図５（ａ），（ｂ）あるいは図６（ａ），（ｂ）に示す如き撮像画像が得られた場合を想定する。図５（ａ），（ｂ）は、ロボット１の進行方向前方に昇り階段が存在する状況でのカメラ３Ｒの撮像画像を例示しており、図６（ａ），（ｂ）は、ロボット１の進行方向前方に降り階段が存在する状況でのカメラ３Ｒの撮像画像を例示している。
【００８９】
この場合、カメラ３Ｒの撮像画像において、図５（ａ），（ｂ）で参照符号Ａr1，Ａr2を付した領域、あるいは、図６（ａ），（ｂ）で参照符号Ａr3，Ａr4，Ａr5，Ａr6を付した領域が平坦面部投影領域として抽出される。
【００９０】
補足すると、２つの撮像画像（ステレオ画像）から空間的な位置データが算出された各部の高さは、例えば、カメラ座標系Ｃcamでの位置データを、ロボット１の支持脚側の接地面（該脚体の足平部と床面との接触面）に設定される座標系に座標変換することで、認識することができる。この場合、座標変換は、ロボット１の各関節の変位量の検出値又は目標値に応じて行なえばよい。
【００９１】
次いで、平坦面部認識手段１１は、抽出した平坦面部投影領域に対応する平坦面部を包含する平面の空間的な位置及び姿勢（法線方向）を表す平面パラメータを算出する。この平面パラメータは、本実施形態では、平面の法線方向の単位ベクトル↑ｎを、カメラ３Ｒ，３Ｌのうちの一方、例えばカメラ３Ｒの光学中心Ｃ0と平面との間の距離ｄ（光学中心Ｃ0から平面への垂線の長さ）により除算してなるベクトル↑ｍ（＝↑ｎ／ｄ）である。以降、この↑ｍを平面パラメータベクトル↑ｍという。なお、本明細書では、“↑”をベクトルを表現する記号として使用している。
【００９２】
上記平面パラメータベクトル↑ｍは、公知の手法によって算出される。例えば、平坦面部投影領域の各部に対応する空間的な位置データを用いて、↑ｍを算出することができる。あるいは、例えば、カメラ３Ｒ，３Ｌにより撮像した２つの撮像画像（ステレオ画像）から、本願出願人が特開２００８−２５０５１６号公報にて先に提案した手法のように、射影変換（平面射影変換）を利用した探索的な演算処理によって↑ｍを算出することもできる。
【００９３】
このようにして平坦面部認識手段１１によって算出される平面パラメータベクトル↑ｍの例を図４（ａ），（ｂ）に示す。図４（ａ），（ｂ）は、例えば図１（ａ）に示した状況で、階段５０（昇り階段）をカメラ３Ｒ，３Ｌにより撮像した場合の例を示しており、図中の↑ｍａ、↑ｍｂが平坦面部認識手段１１の処理によって算出される平面パラメータベクトルである。↑ｍａは、階段５０の上段側の平坦面部５１ａを包含する上段側平面ＰＬａに対応する平面パラメータベクトル、↑ｍｂは、上段側の平坦面部５１ａの一段下側の下段側の平坦面部５１ｂを包含する下段側平面ＰＬｂに対応する平面パラメータである。
【００９４】
なお、図４（ｂ）中のｄａ，ｄｂは、それぞれカメラ３Ｒの光学中心Ｃ0と、上段側平面ＰＬａ、下段側平面ＰＬｂとの距離を示しており、上段側平面ＰＬａ、下段側平面ＰＬｂのそれぞれの法線方向の単位ベクトルをそれぞれ↑ｎａ、↑ｎｂとおくと、↑ｍａ＝↑ｎａ／ｄａ、↑ｍｂ＝↑ｎｂ／ｄｂとなる。
【００９５】
図１（ｂ）に示した状況で、階段５０（降り階段）をカメラ３Ｒ，３Ｌにより撮像した場合においても、上記と同様に、図３（ａ），（ｂ）に示した平面パラメータベクトル↑ｍａ，↑ｍｂが平坦面部認識手段１１の処理によって算出される。
【００９６】
補足すると、平面パラメータは、平面の空間的な位置および姿勢（法線方向）を表すものであればどのようなものでもよい。例えば、平面パラメータベクトル↑ｍの代わりに、平面の法線方向の単位ベクトル↑ｎと、光学中心Ｃ0から平面までの距離ｄとの組（↑ｎ，ｄ）を平面パラメータとして用いてもよい。あるいは、例えば平面上の３点の空間的な位置の組を平面パラメータとして用いることもできる。
【００９７】
以上が平坦面部認識手段１１の処理の詳細である。
【００９８】
次に、段差エッジ特定手段１２に関して説明する。段差エッジ特定手段１２は、ロボット１の進行方向前方に階段等の段差部が存在する場合に、該段差部の各段の平坦面部（ロボット１の遊脚側の脚体の足平部を着地させようとする平坦面部）の実際の段差エッジの位置及び方向を表す段差エッジパラメータを探索的に特定する演算処理を実行することで、段差部の段差エッジの空間的な（３次元空間中の）位置及び方向を推定する。この場合、段差エッジパラメータを探索的に特定する演算処理では、段差エッジパラメータを暫定的に仮決定し、その暫定的に仮決定した段差エッジパラメータを後述する評価関数Ｅ１の値に基づいて適宜修正することで、実際の段差エッジの位置及び方向の推定値を表す段差エッジパラメータとしての最適段差エッジパラメータを特定する。
【００９９】
上記段差エッジパラメータは、それによって、段差エッジの空間的な位置及び方向を規定できるものであればどのようなものでもよい。本実施形態では、例えば、段差エッジを形成する直線（段差エッジに重なる直線）の空間的な方向を示すベクトル（段差エッジと同方向のベクトル）と、該直線上の１つの点の空間的な位置との組、あるいは、段差エッジを形成する直線上の互いに異なる２つの点の空間的な位置の組により構成される。
【０１００】
なお、例えば、段差エッジを形成する空間的な直線を、カメラ３Ｒ，３Ｌのいずれか一方の撮像画像に投影することによって該撮像画像中に得られる二次元直線の、該撮像画像上での二次元的な位置及び方向を表すパラメータ（例えば、該二次元直線の二次元的な方向を示すベクトルと、該二次元直線上の１つの点の位置との組、もしくは、該二次元直線上の互いに異なる２つの点の空間的な位置の組）を、段差エッジパラメータとして採用してもよい。段差エッジは、上段側の平坦面部と同じ平面上に存在するので、該段差エッジの撮像画像中での二次元的な位置及び方向が決まれば、その位置及び方向と、上段側の平坦面部を包含する平面（上段側平面ＰＬａ）の空間的な位置及び姿勢を表す平面パラメータ（例えば前記平面パラメータベクトル）とから、段差エッジの空間的な位置及び姿勢を特定することが可能である。
【０１０１】
ここで、ロボット１の進行方向前方に階段等の段差部が存在する場合に、段差エッジ特定手段１２が、実際の段差エッジに対応する段差エッジパラメータ（最適段差エッジパラメータ）を探索的に特定するために実行する演算処理は、カメラ３Ｒ，３Ｌの視線方向（カメラ３Ｒ，３Ｌから段差部を見る向き）が、段差部の下段側の平坦面部が手前側、上段側の平坦面部が奥側となる方向である場合（例えば図１（ａ）に示す場合。以降、カメラ視線が下・上方向の場合ということがある）と、カメラ３Ｒ，３Ｌの視線方向が、段差部の上段側の平坦面部が手前側、下段側の平坦面部が奥側となる方向である場合（例えば図１（ｂ）に示す場合。以降、カメラ視線が上・下方向の場合ということがある）とで一部の処理が異なる。
【０１０２】
これは、カメラ視線が下・上方向の場合には、上段側の平坦面部とその一段下側の平坦面部との間の蹴込みの箇所の画像が各撮像画像内に含まれることとなる一方、カメラ視線が上・下方向の場合には、蹴込みの箇所の画像が各撮像画像内に含まれないこととなるからである。
【０１０３】
まず、カメラ視線が下・上方向の場合の段差エッジ特定手段１２の処理を説明する。この説明においては、段差部が階段５０であって、カメラ３Ｒの撮像画像が、図５（ａ），（ｂ）に示したような画像となる場合を主要例として段差エッジ特定手段１２の処理を説明する。この場合、図５（ａ），（ｂ）に示す平坦面部投影領域Ａr1，Ａr2にそれぞれ対応する階段５０の平坦面部は、それぞれ、図４（ａ），（ｂ）に示す上段側の平坦面部５１ａ、その一段下側の下段側の平坦面部５１ｂであるとする。なお、以降の説明では、カメラ３Ｒ，３Ｌの撮像画像のうちの一方、例えば、カメラ３Ｒの撮像画像を第１の撮像画像、カメラ３Ｌの撮像画像を第２の撮像画像と称する。また、以降の説明では、暫定的に仮決定する段差エッジパラメータを暫定段差エッジパラメータ、該暫定段差エッジパラメータにより位置及び方向が規定される直線（暫定的な段差エッジを形成する直線）を暫定段差エッジラインＬｅと称する。
【０１０４】
カメラ視線が下・上方向の場合、段差エッジ特定手段１２は、図７のブロック図に示す処理を実行するとで、実際の段差エッジに対応する段差エッジパラメータ（最適段差エッジパラメータ）を探索的に特定する。
【０１０５】
段差エッジ特定手段１２は、まず、Ｓ１の処理を実行し、暫定段差エッジパラメータの初期値を設定する。この場合、暫定段差エッジパラメータの初期値により規定される暫定段差エッジラインＬｅが、ロボット１の進行方向に対して交差する方向（進行方向を横切る方向）に延在すると共に、該暫定段差エッジラインＬｅが、カメラ３Ｒ，３Ｌによって撮像された階段５０の上段側の平坦面部５１ａ（推定しようとする段差エッジを先端に有する平坦面部５１ａ）のうちの、平坦面部認識手段１１によって抽出された平坦面部投影領域に対応する領域を通るように、暫定段差エッジパラメータの初期値が設定される。
【０１０６】
例えば、図４（ａ）に二点鎖線で示す直線Ｌe(0)が、暫定段差エッジパラメータの初期値により規定される暫定段差エッジラインＬｅとされる。この場合、第１の撮像画像上においては、上記暫定段差エッジラインＬｅ（＝Ｌe(0)）を第１の撮像画像に投影してなる線（二次元直線）は、図５（ａ）に示すラインＬep(0)となる。なお、以降、暫定段差エッジラインＬｅを第１の撮像画像に投影してなる二次元直線を、画像上暫定段差エッジラインＬepという。
【０１０７】
上記暫定段差エッジパラメータの初期値のより具体的な設定手法としては、例えば次のような手法が挙げられる。その１つの手法では、例えば、階段５０の上段側の平坦面部５１ａに対応して平坦面部認識手段１１によって抽出された平坦面部投影領域Ａr1の内部に１つの点Ｆ１を設定し、その点Ｆ１に対応する空間上の位置の点（上段側の平坦面部５１ａの点）から下段側の平坦面部５１ｂを包含する平面ＰＬｂに降ろした垂線を第１の撮像画像に投影してなる線Ｆ２を求める。さらに、第１の撮像画像中で、当該投影線Ｆ２に直交し、且つ、上記点Ｆ１を通る直線Ｆ３を求める。そして、暫定段差エッジパラメータの初期値により規定される画像上暫定エッジラインＬep(0)が、上記直線Ｆ３に合致するように、暫定段差エッジパラメータの初期値を設定する。
【０１０８】
あるいは、他の１つの手法では、第１の撮像画像中で、階段５０の上段側の平坦面部５１ａに対応する平坦面部投影領域Ａr1内で、ロボット１の進行方向に延在し、横方向に間隔を有する２つの所定長の線分Ｇ１，Ｇ２を設定し、それらの線分Ｇ１，Ｇ２の中点を結ぶ直線Ｇ３を求める。そして、暫定段差エッジパラメータの初期値により規定される画像上暫定エッジラインＬep(0)が、上記直線Ｇ３に合致するように、暫定段差エッジパラメータの初期値を設定する。
【０１０９】
なお、暫定段差エッジパラメータの初期値の設定の仕方は、上記の例に限らず、種々様々な手法を採用できる。
【０１１０】
補足すると、本実施形態では、段差エッジ特定手段１２は、第１の撮像画像に、図５（ａ），（ｂ）に破線で示す如き方形状の枠内領域ＡＲ１を画像処理領域として設定し、この画像処理領域ＡＲ１での撮像画像を、実際の段差エッジに対応する段差エッジパラメータを探索的に特定するための演算処理に利用する。この場合、第１の撮像画像において、画像処理領域ＡＲ１の上部が、階段５０の上段側の平坦面部５１ａに対応する平坦面部投影領域Ａr1に存在し、且つ、画像処理領域ＡＲ１の下部が、階段５０の下段側の平坦面部５１ｂの平坦面部投影領域Ａr2に存在するように、該画像処理領域ＡＲ１が第１の撮像画像中に設定される。なお、画像処理領域ＡＲ１の横方向の幅は、例えば、概ねロボット１の通過領域と同程度の幅となるように設定される。
【０１１１】
そして、暫定段差エッジパラメータの初期値は、それにより規定される画像上暫定段差エッジラインＬep(0)の画像処理領域ＡＲ１内の部分が、図５（ａ）に示す如く、階段５０の上段側の平坦面部５１ａに対応する平坦面部投影領域Ａr1内に含まれるように設定することが望ましい。
【０１１２】
なお、Ｓ１の処理では、後に、暫定段差エッジパラメータの更新処理も行なうが、これについては後述する。
【０１１３】
段差エッジ特定手段１２は、次に、図７のＳ２の処理と、Ｓ３の処理とを実行する。このうち、Ｓ２の処理では、段差エッジ特定手段１２は、Ｓ１で設定した暫定段差エッジパラメータにより規定される暫定段差エッジラインＬｅを含む（該暫定段差エッジラインＬｅで上段側の平坦面部５１ａを包含する上段側平面ＰＬａと交差する）と共に、上段側の平坦面部５１ａを包含する上段側平面ＰＬａに対して起立した姿勢となる起立平面ＰＬｃ（図４（ａ），（ｂ）を参照）の位置及び姿勢を表す平面パラメータを設定する。
【０１１４】
この場合、起立平面ＰＬｃの平面パラメータは、上段側平面ＰＬａ及び下段側平面ＰＬｂの平面パラメータとしての平面パラメータベクトル↑ｍａ，↑ｍｂと同様に、起立平面ＰＬｃの法線方向の単位ベクトル↑ｎｃを、カメラ３Ｒの光学中心Ｃ0と該起立平面ＰＬｃとの距離ｄｃ（図４（ｂ）を参照）により除算してなる平面パラメータベクトル↑ｍｃ（＝↑ｎｃ／ｄｃ）である。
【０１１５】
ここで、起立平面ＰＬｃは、階段の段鼻の先端面を包含する平面に相当するものである。そして、階段の各段の段鼻の先端面は、一般に、その段の平坦面部（踏面）に対してほぼ垂直に起立した面となっている。
【０１１６】
そこで、本実施形態では、段差エッジ特定手段１２は、Ｓ２において、起立平面ＰＬｃが上段側平面ＰＬａに対して垂直に起立し、且つ、Ｓ１で設定した暫定段差エッジパラメータにより規定される暫定段差エッジラインＬｅを含むように、起立平面パラメータベクトル↑ｍｃを設定する。
【０１１７】
例えば、暫定段差エッジラインＬｅが図４（ａ）に示したＬe(0)である場合、図４（ａ），（ｂ）に示すように、起立平面パラメータベクトル↑ｍｃが設定される。
【０１１８】
この場合、上記起立平面ＰＬｃの空間的な位置及び姿勢（カメラ座標系Ｃcamでの位置及び姿勢）は、カメラ座標系Ｃcamでの上段側平面ＰＬａの姿勢と、暫定段差エッジラインＬｅの位置及び姿勢により一義的に定まることとなる。そこで、Ｓ２の処理では、段差エッジ特定手段１２は、Ｓ１で設定した暫定段差エッジパラメータと、平坦面部認識手段１１で先に算出した上段側平面パラメータベクトル↑ｍａとに応じて、起立平面パラメータベクトル↑ｍｃを設定する。
【０１１９】
なお、上段側平面ＰＬａに対する起立平面ＰＬｃの傾斜角度は、必要に応じて適宜、９０°以外の角度に設定してもよい。例えば、段差エッジを推定しようとする階段の各段鼻の先端面が、踏面に対してほぼ同じ傾斜角度で傾斜した姿勢となっており、その傾斜角度（平均的な傾斜角度）が事前の計測等により判明しているような場合には、その傾斜角度だけ、起立平面ＰＬｃが上段側平坦面ＰＬａに対して傾斜させた姿勢となるように、起立平面パラメータを設定するようにしてもよい。
【０１２０】
前記Ｓ３の処理では、段差エッジ特定手段１２は、第１の撮像画像中に、後述の射影変換（平面射影変換）を行なうために用いる３つの領域Ａa1，Ａb1，Ａc1を、Ｓ１で設定した暫定段差エッジパラメータに応じて設定する。
【０１２１】
第１の撮像画像中の３つの領域Ａa1，Ａb1，Ａc1は、第１の撮像画像中に前記した如く設定した画像処理領域ＡＲ１内で図５（ａ），（ｂ）に例示する如く設定される。これらの３つの領域Ａa1，Ａb1，Ａc1は、それぞれ、本発明における第２の領域、第３の領域、第１の領域に相当するものであり、これらの設定の仕方の基本的な指針は次の通りである。
【０１２２】
すなわち、領域Ａa1，Ａc1については、基本的には、第１の撮像画像において、画像上暫定段差エッジラインＬepが領域Ａa1，Ａc1の境界線となると共に、領域Ａc1が、上段側の平坦面部５１ａと下段側の平坦面部５１ｂとの間の蹴込みの箇所（これは、本発明における上下段間部分に相当する）と、上段側の平坦面部５１ａとのうちの蹴込みの箇所寄りの領域、領域Ａa1が、上段側の平坦面部５１ａ寄りの領域となるように、これらの領域Ａa1，Ａc1の画像処理領域ＡＲ１内で設定される。
【０１２３】
従って、図５（ａ），（ｂ）に示す第１の撮像画像においては、領域Ａa1，Ａc1は、画像処理領域ＡＲ１内で、画像上暫定段差エッジラインＬep（図５（ａ）ではＬep(0)、図５（ｂ）ではＬep(n）)を間に挟んで上下に隣接するように設定される。
【０１２４】
この場合、領域Ａa1の上側の境界線は、画像処理領域ＡＲ１の上端の境界線に一致するものとされる。すなわち、画像処理領域ＡＲ１の画像上暫定段差エッジラインＬepの上側の全体の領域が、領域Ａa1として設定される。
【０１２５】
一方、領域Ａc1の下側の境界線（画像上暫定段差エッジラインＬep（Ｌep(0)，Ｌep(n)）と反対側の境界線）については、図５（ａ），（ｂ）に例示する如く、画像上暫定段差エッジラインＬepと所定の間隔Δｃを存して平行に延在するように設定される。その間隔Δｃは、本実施形態では、段鼻が踏面の先端部に形成された一般的な階段をカメラ３Ｒ，３Ｌにより撮像した場合に、それぞれの撮像画像における段鼻の厚みとほぼ同程度になるようにあらかじめ設定されている。
【０１２６】
なお、撮像画像における段鼻の厚みは、カメラ３Ｒ，３Ｌから段鼻への距離によって変化するので、例えば、暫定段差エッジラインの近辺での上段側の平坦面部５１ａの距離（カメラ３Ｒ又は３Ｌからの距離）に応じて、領域Ａc1の上記間隔Δｃを可変的に設定するようにしてもよい。
【０１２７】
また、領域Ａb1については、基本的には、第１の撮像画像において、領域Ａc1よりも下段側の平坦面部５１ｂ寄りの位置で該下段側の平坦面部５１ｂの少なくとも一部分を含むと共に、領域Ａb1の領域Ａc1側の境界線の第１の撮像画像中での位置及び方向が、画像上暫定段差エッジラインＬepの位置及び方向に応じて規定されるように、該領域Ａb1が画像処理領域ＡＲ１内で設定される。
【０１２８】
この場合、領域Ａb1の上側の境界線は、領域Ａc1の下側の境界線と所定の間隔Δｂを存して平行に延在するように設定される。その間隔Δｂは、本実施形態では、画像上暫定段差エッジラインＬepが、第１の撮像画像における実際の段差エッジ５２の線に一致もしくはほぼ一致する場合（図５（ｂ）に示す場合）に、階段５０の上段側の平坦面部５１ａと下段側の平坦面部５１ｂとの間の蹴込みの箇所が、領域Ａb1から除外されると共に、領域Ａb1の領域Ａc1側の境界線が下段側の平坦面部５１ｂの画像内に存在するようにあらかじめ設定されている。
【０１２９】
なお、領域Ａc1に係わる上記間隔Δｃと同様に、領域Ａb1に係わる上記間隔Δｂを、例えば、下段側の平坦面部５１ｂの奥側部分の距離（カメラ３Ｒ又は３Ｌからの距離）に応じて可変的に設定するようにしてもよい。
【０１３０】
そして、領域Ａb1の下側の境界線は、画像処理領域ＡＲ１の下端の境界線に一致するものとされる。すなわち、画像処理領域ＡＲ１のうち、上記の如く設定される領域Ａb1の上側の境界線の下側の全体の領域が、領域Ａb1として設定される。
【０１３１】
なお、本実施形態では、上記Ｓ３の処理によって、本発明における評価関数算出用領域設定手段を実現される。
【０１３２】
以上の如く、Ｓ２、Ｓ３の処理を実行した後、段差エッジ特定手段１２は、次に、Ｓ４、Ｓ５、Ｓ６の処理を実行する。このＳ４、Ｓ５、Ｓ６では、段差エッジ特定手段１２は、それぞれ、Ｓ３で設定した第１の撮像画像の領域Ａa1、領域Ａb1、領域Ａc1にそれぞれ対応する第２の撮像画像中の領域の画像に射影変換（平面射影変換）を施した画像を算出する。以降の説明では第１の撮像画像の領域Ａa1、領域Ａb1、領域Ａc1にそれぞれ対応する第２の撮像画像中の領域を、それぞれ、領域Ａa2、領域Ａb2、領域Ａc2と表記する。
【０１３３】
ここで、上記射影変換は、一般的に言えば、ステレオ画像を構成する２つの撮像画像に、階段の踏面の如き平坦面部（平面部分）を撮像した場合に、一方の撮像画像中における当該平坦面部の画像を、他方の撮像画像における当該平坦面部の画像に変換するものである。その変換を行なう射影変換行列Ｐは、カメラ３Ｒ，３Ｌの２つの撮像画像のそれぞれの画像座標（各撮像画像における各画素の位置を表す二次元座標）を同次座標で表現した場合に、公知のように次式（１−１）により表される行列である。
【０１３４】
Ｐ＝Ｒ＋↑ｔ・↑ｍT ……（１−１）
式（１−１）におけるＲは、カメラ３Ｒ，３Ｌのそれぞれのカメラ座標系の間の回転行列、↑ｔはそれらのカメラ座標系の間の並進移動ベクトル（ここでは縦ベクトル）、↑ｍは前記した如く定義される平面パラメータベクトル（ここでは縦ベクトル）であり、↑ｍTは、↑ｍの転置を意味する。
【０１３５】
この場合、回転行列Ｒ及び並進移動ベクトル↑ｔは、カメラ３Ｒ，３Ｌの相互の相対的な位置関係及び姿勢関係に依存して定まるものであり、本実施形態ではあらかじめ特定されている。
【０１３６】
補足すると、第１の撮像画像から第２の撮像画像への射影変換行列と、これの逆変換の射影変換行列（第２の撮像画像から第１の撮像画像への射影変換行列）とは、いずれも、式（１−１）の右辺の形で表現されるものの、上記Ｒ，↑ｔ，↑ｍの値は、それぞれの射影変換行列で異なる。
【０１３７】
具体的には、式（１−１）により表現される射影変換行列Ｐを第１の撮像画像及び第２の撮像画像のうちの一方から他方へのへの射影変換行列とした場合、その逆変換の射影変換行列（以降、これをＰ-1と表記する）は、次式（１−２）の但し書きの式（１−２ａ）〜（１−２ｄ）により定義されるＲ’、↑ｔ’、↑ｍ’、↑ｄ’を用いて、上記式（１−１）と同様の形の式（１−２）により表される。
【０１３８】
Ｐ-1＝Ｒ'＋↑ｔ’・(↑ｍ’)T ……（１−２）
但し、
Ｒ’≡ＲT ……（１−２ａ）
↑ｔ’≡−ＲT・↑ｔ ……（１−２ｂ）
↑ｍ’≡Ｒ・↑ｍ／ｄ’ ……（１−２ｃ）
↑ｄ’≡ｄ・（１＋↑ｍT・ＲT・↑ｔ） ……（１−２ｄ）
以降の本実施形態の説明では、便宜上、第１の撮像画像から第２の撮像画像への射影変換行列を式（１−１）により表される射影変換行列Ｐとし、第２の撮像画像から第１の撮像画像への射影変換行列を式（１−２）により表される射影変換行列Ｐ-1とする。
【０１３９】
以上を前提としてＳ４，Ｓ５，Ｓ６の処理を、図８を参照しつつ以下に具体的に説明する。なお、図８は、Ｓ４、Ｓ５，Ｓ６の処理を包括的に説明するための図であり、図中の符号中の「ｘ」は、Ｓ４の処理では「ａ」、ｓ５の処理では「ｂ」、Ｓ６の処理では「ｃ」を意味するものであるとする。
【０１４０】
Ｓ４では、段差エッジ特定手段１２は、平坦面部認識手段１１により先に算出された上段側平面ＰＬａの平面パラメータベクトル↑ｍａを式（１−１）の↑ｍに代入することで射影変換行列Ｐａを決定し、このＰａを用いて、第１の撮像画像の領域Ａa1に対応する第２の撮像画像中の領域Ａa2を求める。具体的には、図８に示す如く、第１の撮像画像の領域Ａa1の外形枠にＰａによる射影変換を施すことによって、第２の撮像画像中の領域Ａa2を求める。
【０１４１】
さらに、段差エッジ特定手段１２は、前記式（１−２）及びそのただし書きの式に基づいて、上記Ｐａの逆変換の射影変換行列Ｐａ-1を決定し、このＰａ-1による射影変換を第２の撮像画像中の領域Ａa2内の画像に対して施す。これにより、第１の撮像画像に設定した領域Ａa1に対応する第２の撮像画像の領域Ａa2に、上段側平面ＰＬａの平面パラメータベクトル↑ｍａに応じた射影変換を施してなる画像が算出される。
【０１４２】
上記と同様に、Ｓ５では、段差エッジ特定手段１２は、平坦面部認識手段１１により先に算出された下段側平面ＰＬｂの平面パラメータベクトル↑ｍｂを式（１−１）の↑ｍに代入することで射影変換行列Ｐｂを決定し、このＰｂによる射影変換を第１の撮像画像の領域Ａb1の外形枠に施すことによって、Ａb1に対応する第２の撮像画像中の領域Ａb2を求める。そして、段差エッジ特定手段１２は、上記Ｐｂの逆変換の射影変換行列Ｐｂ-1を決定し、このＰｂ-1による射影変換を第２の撮像画像中の領域Ａb2内の画像に対して施す。これにより、第１の撮像画像に設定した領域Ａb1に対応する第２の撮像画像の領域Ａb2に、下段側平面ＰＬｂの平面パラメータベクトル↑ｍｂに応じた射影変換を施してなる画像が算出される。
【０１４３】
また、Ｓ６では、Ｓ２の処理により算出した起立平面ＰＬｃの平面パラメータベクトル↑ｍｃを式（１−１）の↑ｍに代入することで射影変換行列Ｐｃを決定し、このＰｃによる射影変換を第１の撮像画像の領域Ａc1の外形枠に施すことによって、Ａc1に対応する第２の撮像画像中の領域Ａc2を求める。そして、段差エッジ特定手段１２は、上記Ｐｃの逆変換の射影変換行列Ｐｃ-1を決定し、このＰｂ-1による射影変換を第２の撮像画像中の領域Ａc2内の画像に対して施す。これにより、第１の撮像画像に設定した領域Ａc1に対応する第２の撮像画像の領域Ａc2に、起立平面ＰＬｃの平面パラメータベクトル↑ｍｃに応じた射影変換を施してなる画像が算出される。
【０１４４】
補足すると、本実施形態では、Ｓ４、Ｓ５、Ｓ６のそれぞれの処理において、第１の撮像画像に設定した各領域Ａx1（ｘ＝ａ又はｂ又はｃ。以降、同様）に対応する第２の撮像画像中の領域Ａx2を、平面パラメータベクトル↑ｍｘに応じて決定した射影変換行例Ｐｘによる射影変換によって算出した上で、第２の撮像画像中の領域Ａx2内の画像に、Ｐｘの逆変換の射影変換行列Ｐｘ-1による射影変換を施すことによって、第１の撮像画像の領域Ａx1に対応する第２の撮像画像中の領域Ａx2の画像の射影変換画像を求めるようにした。
【０１４５】
但し、第１の撮像画像の各領域Ａx1に対応する第２の撮像画像中の領域Ａx2そのものを求めたり、射影変換行列Ｐｘ-1による射影変換を第２の撮像画像に対して実際に行なうことを必須とするものではなく、結果的に、第１の撮像画像の領域Ａx1に対応する第２の撮像画像中の領域Ａx2の画像の射影変換画像を求めることができる処理であれば、上記の手法と異なる手法を採用してもよい。
【０１４６】
例えば、図９を参照して、第１の撮像画像中の各領域Ａx1内の各画素位置Ｑ１に対応する第２の撮像画像上の画素位置Ｑ２を、平面パラメータベクトル↑ｍｘに応じて決定した射影変換行例Ｐｘによる射影変換によって算出し、第１の撮像画像中の領域Ａx1の各画素位置Ｑ１の画素値を、その画素位置に対応して算出した第２の撮像画像上の画素位置Ｑ２における該第２の撮像画像の画素値で置き換える。このようにしても、結果的に、第１の撮像画像の領域Ａx1に対応する第２の撮像画像中の領域Ａx2の画像の射影変換画像を得ることができる。従って、第１の撮像画像の各領域Ａx1に対応する第２の撮像画像中の領域Ａx2そのものを求めたり、射影変換行列Ｐｘ-1による射影変換を第２の撮像画像に対して実際に行なうことを実行せずとも、第１の撮像画像の領域Ａx1に対応する第２の撮像画像中の領域Ａx2の画像の射影変換画像を得ることができる。
【０１４７】
以上が、本実施形態におけるＳ４、Ｓ５、Ｓ６の処理である。
【０１４８】
ここで、暫定段差エッジパラメータにより規定される暫定段差エッジラインＬｅが、実際の段差エッジに合致する場合には、第１の撮像画像の領域Ａa1，Ａb1，Ａc1は、それぞれ、その全体もしくは大部分が、上段側平面ＰＬａ上の部分の画像、下段側平面ＰＬｂ上の部分の画像、起立平面ＰＬｃ上の部分の画像に相当するものとなる。これは、領域Ａa1，Ａb1，Ａc1を前記した如く設定することによって、領域Ａa1，Ａb1，Ａc1は、暫定段差エッジパラメータにより規定される暫定段差エッジラインＬｅが、実際の段差エッジに合致している場合に、それぞれ、上段側の平坦面部５１ａの部分の画像、下段側の平坦面部５１ｂの部分の画像、段鼻の先端面の部分の画像となるように設定されるからである（図５（ｂ）を参照）。
【０１４９】
このため、暫定段差エッジラインＬｅが、実際の段差エッジに合致する場合には、第１の撮像画像の領域Ａa1，Ａb1，Ａc1にそれぞれ対応する第２の撮像画像中の領域Ａa2，Ａb2，Ａc2の各画像を射影変換した場合に得られる画像（以降、射影変換画像という）は、それぞれ、第１の撮像画像の領域Ａa1，Ａb1，Ａc1のそれぞれの実際の画像に一致もしくはほぼ一致することとなる。
【０１５０】
一方、暫定段差エッジラインＬｅが、実際の段差エッジと合致していない場合には、第２の撮像画像の領域Ａa2，Ａb2，Ａc2の各画像の射影変換画像は、それぞれ、第１の撮像画像の領域Ａa1，Ａb1，Ａc1の実際の画像に対して差異を生じることとなる。そして、その差異の度合いは、暫定段差エッジラインＬｅと実際の段差エッジとの一致度合いに応じて変化する。
【０１５１】
そこで、本実施形態では、段差エッジ特定手段１２は、以下に説明するＳ７〜Ｓ１０の処理によって、暫定段差エッジラインＬｅと実際の段差エッジとの一致度合いに応じて値が変化する所定の評価関数を、第１の撮像画像の領域Ａa1，Ａb1，Ａc1の各画像の画素値データと、第２の撮像画像の領域Ａa2，Ａb2，Ａc2の各画像の射影変換画像の画像値データとから算出する。そして、段差エッジ特定手段１２は、この評価関数によって暫定段差エッジラインＬｅと実際の段差エッジとの一致度合いを評価し、その評価結果に応じて暫定段差エッジパラメータを適宜修正することによって、該評価関数の値を最小化する段差エッジパラメータ（評価関数最小化段差エッジパラメータ）を探索的に特定する。このようにして特定される評価関数最小化段差エッジパラメータが、実際の段差エッジの位置及び方向を表す段差エッジパラメータとして得られる。
【０１５２】
上記Ｓ７〜Ｓ１０の処理を具体的に説明すると、段差エッジ特定手段１２は、Ｓ４〜Ｓ６の処理に続いて、Ｓ７〜Ｓ９の処理を実行する。Ｓ７では、段差エッジ特定手段１２は、第１の撮像画像の領域Ａa1の画像の画素値データと、第２の撮像画像の領域Ａa2の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Ｄａ１を次式（２）により算出する。
【０１５３】
また、Ｓ８では、段差エッジ特定手段１２は、第１の撮像画像の領域Ａb1の画像の画素値データと、第２の撮像画像の領域Ａb2の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Ｄｂ１を次式（３）により算出する。
【０１５４】
また、Ｓ９では、段差エッジ特定手段１２は、第１の撮像画像の領域Ａc1の画像の画像値データと、第２の撮像画像の領域Ａc2の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Ｄｃ１を次式（４）により算出する。
【０１５５】
これらの誤差関数Ｄａ１，Ｄｂ１，Ｄｃ１はそれぞれ、本発明における第２誤差関数、第３誤差関数、第１誤差関数に相当するものである。
【０１５６】
【数１】
式（２）、（３）、（４）において、Ｉ1[u]は、第１の撮像画像の座標位置ｕでの画素値、Ｉ2[w(u ;↑mx)]（但し、↑ｍｘ＝↑ｍａ又は↑ｍｂ又は↑ｍｃ）は、第２の撮像画像の↑ｍｘに応じた射影変換によってから得られる射影変換画像において、第１の撮像画像の座標位置ｕに対応する座標位置w(u ;↑mx)での画素値である。また、式（２）におけるΣ、式（３）におけるΣ、式（４）におけるΣは、それぞれ、第１の撮像画像の領域Ａa1，Ａb1，Ａc1における積算演算を意味する。
【０１５７】
なお、画素値Ｉ1[u]，Ｉ2[w(u ;↑mx)]は、一般的には、各画素の輝度値でよいが、第１の撮像画像及び第２の撮像画像が、カラー画像である場合には、彩度や色相等を使用することもできる。
【０１５８】
また、式（２）のＮa1、式（３）のＮb1、式（４）のＮc1はそれぞれ、領域Ａa1，Ａb1，Ａc1の画素数である。
【０１５９】
上記式（２）により算出される誤差関数Ｄａ１は、第１の撮像画像の領域Ａc1の画像の画素値分布と、第２の撮像画像の領域Ａc2の画像の射影変換画像の画素値分布とが一致していれば最小となり、両画素値分布の誤差が大きいほど、誤差関数Ｄａ１の値が大きくなる。このことは、誤差関数Ｄｂ１、Ｄｃ１についても同様である。
【０１６０】
なお、本実施形態では、誤差関数Ｄａ１を、Ｉ1[u]とＩ2[w(u ;↑ma)]との差の２乗値を領域Ａa1で積算したものとしたが、誤差関数Ｄａ１は、第１の撮像画像の領域Ａa1の画像の画素値分布と、第２の撮像画像の領域Ａa2の画像の射影変換画像の画素値分布との誤差の程度を表現できるものであれば、その他の形態のものであってもよい。例えば、Ｉ1[u]とＩ2[w(u ;↑ma)]との差の絶対値を領域Ａa1で積算したものを誤差関数Ｄａ１として使用してもよい。このことは、誤差関数Ｄｂ１，Ｄｃ１についても同様である。
【０１６１】
次いで、段差エッジ特定手段１２は、Ｓ１０の処理を実行する。このＳ１０では、段差エッジ特定手段１２は、Ｓ７〜Ｓ９でそれぞれ算出した誤差関数Ｄａ１，Ｄｂ１，Ｄｃ１の値を、次式（５）により線形結合することによって、評価関数Ｅ１の値を算出する。
【０１６２】
Ｅ１＝Ｄａ１＋Ｄｂ１＋Ｄｃ１ ……式（５）
この場合、第１の撮像画像の領域Ａa1，Ａb1，Ａc1は、暫定段差エッジパラメータに依存して設定される領域であるので、上記評価関数Ｅ１も、暫定段差エッジパラメータに依存する（暫定エッジパラメータの関数となる）。そして、前記した如く、暫定段差エッジラインＬｅが、実際の段差エッジに合致する場合には、第１の撮像画像の領域Ａa1，Ａb1，Ａc1の各画像が、それぞれ、第２の撮像画像の領域Ａa2，Ａb2，Ａc2のそれぞれの画像の射影変換画像に一致もしくはほぼ一致するので、誤差関数Ｄａ１，Ｄｂ１，Ｄｃ１の値がそれぞれ最小となる。ひいては、評価関数Ｅ１の値が最小となる。
【０１６３】
なお、式（５）の線形結合は、単純な加算演算であるが、Ｄａ１，Ｄｂ１，Ｄｃ１にそれぞれ重み係数を付して、Ｄａ１，Ｄｂ１，Ｄｃ１を線形結合することによって、評価関数Ｅ１を算出するようにしてもよい。
【０１６４】
上記の如く評価関数Ｅ１を算出した後、段差エッジ特定手段１２は、次に、Ｓ１において、上記評価関数Ｅ１の値を最小化する（“０”に近づける）ように、暫定段差エッジパラメータを更新する。
【０１６５】
この更新処理は、例えば次のように行われる。なお、ここでの説明では、更新前の最新の暫定段差エッジパラメータを更新前暫定段差エッジパラメータ、更新後の暫定段差エッジパラメータを更新後暫定段差エッジパラメータという。
【０１６６】
以下説明すると、段差エッジ特定手段１２は、暫定段差エッジラインＬｅの空間的な位置及び方向の一方又は両方、あるいは、第１の撮像画像における画像上暫定段差エッジラインＬepの二次元的な位置及び方向の一方又は両方を所定の微小量だけ、更新前暫定段差エッジパラメータに対応する値から変更し、その変更後のＬｅ又はＬepに対応する段差エッジパラメータを、観測用段差エッジパラメータとする。
【０１６７】
そして、段差エッジ特定手段１２は、前記画像処理領域ＡＲ１を、図１０に示すように第１の撮像画像の横方向に分割してなる複数の微小領域δＡＲのそれぞれにおいて、上記観測用段差エッジパラメータを更新前暫定段差エッジパラメータの代わりに用いて前記Ｓ３〜Ｓ１０と同様の処理を実行することによって、各微小領域δＡＲ毎の評価関数Ｅ１の値Ｅ１δを算出する。
【０１６８】
そして、段差エッジ特定手段１２は、各微小領域δＡＲ毎の評価関数Ｅ１の値Ｅ１δと、更新前暫定段差エッジパラメータに対応するＳ１０の処理で先に算出した評価関数Ｅ１の値との偏差に応じて、各微小領域δＡＲ毎に、画像上暫定段差エッジラインＬepの二次元的な位置（第１の撮像画像の縦方向での位置）のずらし量（更新前暫定段差エッジパラメータにより規定される画像上段差エッジパラメータＬepからのずらし量）を決定する。そのずらし量は、例えば、上記偏差の符号（極性）に応じて、あるいは、該偏差に所定値のゲインを乗じることで決定される。
【０１６９】
次いで、段差エッジ特定手段１２は、各微小領域δＡＲ毎に、図１０に示す如く、更新前暫定段差エッジパラメータにより規定される画像上暫定段差エッジラインＬepとしての更新前Ｌep上の点（例えば、各微小領域δＡＲでの更新前Ｌepの中央点）から上記ずらし量だけ第１の撮像画像の縦方向にずらした点Ｐδを算出する。
【０１７０】
次いで、段差エッジ特定手段１２は、各微小領域δＡＲ毎の点Ｐδを近似的に結ぶ直線を、更新後暫定段差エッジパラメータにより規定される暫定段差エッジラインＬep（更新後Ｌep）とし、該更新後Ｌepを最小二乗法により算出する。そして、段差エッジ特定手段１２は、この更新後Ｌepに対応する暫定段差エッジパラメータを更新後暫定段差エッジパラメータとして決定する。
【０１７１】
以上が、Ｓ１における暫定段差エッジパラメータの更新処理である。
【０１７２】
なお、暫定段差エッジパラメータの更新の仕方は、上記した手法に限られるものでなく、種々様々な公知の探索手法を利用できる。例えば、観測用段差エッジパラメータを複数種類、設定し、それぞれの観測用段差エッジパラメータを暫定段差エッジパラメータの代わりに用いて、Ｓ２〜Ｓ１０と同じ処理を実行することで、各観測用段差エッジパラメータ毎の評価関数Ｅ１の値を算出する。そして、それらの評価関数Ｅ１の値と、更新前暫定段差エッジパラメータに応じて算出した評価関数Ｅ１の値との偏差を基に、最急降下法等の勾配法の手法によって、評価関数Ｅ１の値を“０”に近づける更新後暫定段差エッジパラメータを決定するようにしてもよい。
【０１７３】
段差エッジ特定手段１２は、以上の如く暫定段差エッジパラメータを更新するＳ１の処理と、それに続くＳ２〜Ｓ１０の処理とを繰り返す。そして、段差エッジ特定手段１２は、ｎ回目のＳ１０の処理で算出した評価関数Ｅ１の値と、ｎ−１回目のＳ１０の処理で算出した評価関数Ｅ１の値とを比較し、それらの値の差の絶対値が、“０”近傍の所定の設定値よりも小さくなった場合にｎ回目のＳ２〜Ｓ１０の処理で使用した暫定段差エッジパラメータを、実際の段差エッジ５２の位置及び方向を表す最適段差エッジパラメータとして決定する。
【０１７４】
これにより、評価関数Ｅ１の値を最小化するように最適段差エッジパラメータが探索的に特定されることとなる。例えば、ｎ回目の暫定段差エッジパラメータにより規定される画像上暫定段差エッジラインＬepが図５（ｂ）に示すＬep(n)である場合、このＬep(n)に対応する暫定段差エッジパラメータが最適段差エッジパラメータとして決定される。
【０１７５】
なお、ｎ回目のＳ１０の処理で算出した評価関数Ｅ１の値が、“０”近傍の所定の設定値よりも小さい場合、あるいは、暫定段差エッジパラメータを更新するＳ１の処理と、それに続くＳ２〜Ｓ１０の処理との繰り返し回数ｎが、所定回数に達した場合に、ｎ回目の処理で使用した暫定段差エッジパラメータを、実際の段差エッジ５２の位置及び方向を表す最適段差エッジパラメータとして決定するようにしてもよい。
【０１７６】
以上が、カメラ視線が下・上方向の場合における段差エッジ特定手段１２の処理である。
【０１７７】
次に、カメラ視線が上・下方向の場合における段差エッジ特定手段１２の処理を説明する。この説明においては、段差部が階段５０であって、カメラ３Ｒの撮像画像（第１の撮像画像）が、図６（ａ），（ｂ）に示したような画像となる場合を主要例として段差エッジ特定手段１２の処理を説明する。この場合、図６（ａ），（ｂ）に示す平坦面部投影領域Ａr3，Ａr4にそれぞれ対応する階段５０の平坦面部は、それぞれ、図４（ａ），（ｂ）に示す上段側の平坦面部５１ａ、その一段下側の下段側の平坦面部５１ｂであるとする。
【０１７８】
カメラ視線が上・下方向の場合には、図６（ａ），（ｂ）に例示した第１の撮像画像のように、カメラ３Ｒ，３Ｌの各撮像画像には、階段５０の段鼻の先端面に相当する面（段差エッジから垂下する面）が写らない。
【０１７９】
このため、この場合には、段差エッジ特定手段１２は、図７のブロック図に示した処理から、起立平面ＰＬｃに関連する処理を省略してなる図１１のブロック図に示す処理によって、実際の段差エッジの位置及び方向の推定値を表す最適段差エッジパラメータを探索的に特定する。
【０１８０】
以下説明すると、段差エッジ特定手段１２は、まず、Ｓ２１の処理を実行し、暫定段差エッジパラメータの初期値を設定する。この暫定段差エッジパラメータの初期値の設定の仕方は、前記Ｓ１の処理と同じでよい。この場合、画像上暫定段差エッジラインＬepが、例えば、図６（ａ）に示したＬep(0)となるような暫定段差エッジパラメータの初期値が設定される。但し、カメラ視線が上・下方向の場合では、暫定段差エッジパラメータの初期値により規定される画像上暫定段差エッジラインＬep(0)が、上段側の平坦面部５１ａに対応する平坦面部投影領域Ａr3から逸脱するように設定されていてもよい。
【０１８１】
次いで、段差エッジ特定手段１２は、Ｓ２２の処理を実行し、第１の撮像画像中の画像処理領域ＡＲ２内に、射影変換（平面射影変換）に用いる２つの領域Ａa3，Ａb3を、Ｓ２１で設定した暫定段差エッジパラメータに応じて設定する。
【０１８２】
ここで、Ｓ２２の処理で使用する画像処理領域ＡＲ２は、図６（ａ），（ｂ）に破線で示すように設定される方形状の枠内領域である。この画像処理領域ＡＲ２は、その下部が、階段５０の上段側の平坦面部５１ａに対応する平坦面部投影領域Ａr1に存在し、且つ、画像処理領域ＡＲの上部が、階段５０の下段側の平坦面部５１ｂの平坦面部投影領域Ａr2に存在するように第１の撮像画像中に設定される。また、この画像処理領域ＡＲ２の横方向の幅は、例えば、概ねロボット１の通過領域と同程度の幅となるように設定される。
【０１８３】
そして、この画像処理領域ＡＲ２内に設定する２つの領域Ａa3，Ａb3はそれぞれ、本発明における第４領域、第５の領域に相当するものであり、これらは、次のように設定される。
【０１８４】
すなわち、第１の撮像画像において、画像上暫定段差エッジラインＬepが領域Ａa3，Ａb3の境界線となると共に、領域Ａa3が、上段側の平坦面部５１ａと下段側の平坦面部５１ｂとのうちの上段側の平坦面部５１ａ寄りの領域、領域Ａb3が下段側の平坦面部５１ｂ寄りの領域となるように、これらの領域Ａa3，Ａb3が設定される。
【０１８５】
図６（ａ），（ｂ）に示す第１の撮像画像においては、画像処理領域ＡＲ２のうちの、画像上暫定段差エッジラインＬep（図６（ａ）ではＬep(0)、図６（ｂ）ではＬep(n）)の下側の領域が領域Ａa3、上側の領域が領域Ａb3として設定される。
【０１８６】
なお、前記Ｓ２２の処理は、本発明における評価関数算出用領域設定手段を実現するものである。
【０１８７】
次いで、段差エッジ特定手段１２は、Ｓ２３、Ｓ２４の処理を実行する。Ｓ２３では、段差エッジ特定手段１２は、図７のＳ４の処理と同様に、上段側平面ＰＬａの平面パラメータベクトル↑ｍａを前記式（１−１）の↑ｍに代入することで決定した射影変換行列Ｐａにより、第１の撮像画像の領域Ａa3の外形枠に射影変換を施すことによって、Ａa3に対応する第２の撮像画像中の領域（以下、これを領域Ａa4という）を決定する。そして、段差エッジ特定手段１２は、Ｐａの逆変換の射影変換行列Ｐａ-1による射影変換を第２の撮像画像中の領域Ａa4の画像に施すことによって、領域Ａa4の射影変換画像を求める。
【０１８８】
また、Ｓ２４では、段差エッジ特定手段１２は、図７のＳ５の処理と同様に、下段側平面ＰＬｂの平面パラメータベクトル↑ｍｂを式（１−１）の↑ｍに代入することで決定した射影変換行列Ｐｂにより、第１の撮像画像の領域Ａb3の外形枠に射影変換を施すことによって、該領域Ａb3に対応する第２の撮像画像中の領域（以下、これを領域Ａb4という）を決定する。そして、段差エッジ特定手段１２は、Ｐｂの逆変換の射影変換行列Ｐｂ-1による射影変換を第２の撮像画像中の領域Ａb4の画像に施すことによって、領域Ａb4の射影変換画像を求める。
【０１８９】
このようにして算出される第２の撮像画像の領域Ａa4，Ａb4の各画像の射影変換画像は、暫定段差エッジラインＬｅが、実際の段差エッジに合致する場合には、それぞれ、第１の撮像画像の領域Ａa3，Ａb3のそれぞれの実際の画像に一致もしくはほぼ一致することとなる。
【０１９０】
そして、暫定段差エッジラインＬｅが、実際の段差エッジと合致していない場合には、第２の撮像画像の領域Ａa4，Ａb4の各画像の射影変換画像は、それぞれ、第１の撮像画像の領域Ａa3，Ａb3の実際の画像に対して差異を生じ、その差異の度合いが、暫定段差エッジラインＬｅと実際の段差エッジとの一致度合いに応じて変化する。
【０１９１】
そこで、段差エッジ特定手段１２は、次に、図７のＳ７〜１０と同様の処理をＳ２５〜Ｓ２７で実行し、暫定段差エッジラインＬｅと実際の段差エッジとの一致度合いに応じて値が変化する評価関数を算出する。
【０１９２】
具体的には、Ｓ２５では、段差エッジ特定手段１２は、第１の撮像画像の領域Ａa3の画像の画像値データと、第２の撮像画像の領域Ａa4の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Ｄａ２を次式（６）により算出する。
【０１９３】
また、Ｓ２６では、段差エッジ特定手段１２は、第１の撮像画像の領域Ａb3の画像の画像値データと、第２の撮像画像の領域Ａb4の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Ｄｂ２を次式（７）により算出する。
【０１９４】
これらの誤差関数Ｄａ２，Ｄｂ２はそれぞれ、本発明における第４誤差関数、第５誤差関数に相当するものである。
【０１９５】
【数２】
式（６）、（７）において、Ｉ1[u]、Ｉ2[w(u ;↑mx)]（但し、↑ｍｘ＝↑ｍａ又は↑ｍｂ）の意味は、前記式（２）〜（４）のものと同じである。また、式（６）におけるΣ、式（７）におけるΣは、それぞれ、第１の撮像画像の領域Ａa3，Ａb3における積算演算を意味する。また、式（６）のＮa2、式（７）のＮb2はそれぞれ、領域Ａa3，Ａb3の画素数である。
【０１９６】
なお、誤差関数Ｄａ２，Ｄｂ２は、前記式（２）〜（４）の誤差関数Ｄａ１，Ｄｂ１，Ｄｃ１に関して説明した場合と同様に、その他の形態のものであってもよい。例えば、Ｉ1[u]とＩ2[w(u ;↑ma)]との差の絶対値を領域Ａa3で積算したものを誤差関数Ｄａ２として使用してもよい。
【０１９７】
次いで、段差エッジ特定手段１２は、Ｓ２７の処理を実行する。このＳ２７では、段差エッジ特定手段１２は、Ｓ２５，２６でそれぞれ算出した誤差関数Ｄａ２，Ｄｂ２の値を、次式（８）により線形結合することによって、評価関数Ｅ２の値を算出する。
【０１９８】
Ｅ２＝Ｄａ２＋Ｄｂ２ ……式（８）
この場合、第１の撮像画像の領域Ａa3，Ａb3が前記したように設定されるので、上記のように算出される評価関数Ｅ２は、その値が、暫定段差エッジパラメータに依存するものとなる。そして、暫定段差エッジラインＬｅが、実際の段差エッジに合致する場合には、誤差関数Ｄａ２，Ｄｂ２の値がそれぞれ最小（≒０）となるので、評価関数Ｅ２の値も最小となる。
【０１９９】
なお、Ｄａ２，Ｄｂ２に重み係数を付して、Ｄａ２，Ｄａ１を線形結合することによって評価関数Ｅ２を算出するようにしてもよい。
【０２００】
上記の如く、評価関数Ｅ２を算出した後、段差エッジ特定手段１２は、次に、Ｓ２１において、上記評価関数Ｅ２の値を最小化する（“０”に近づける）ように、暫定段差エッジパラメータを更新する。そして、段差エッジ特定手段１２は、暫定段差エッジパラメータを更新するＳ２１の処理と、それに続くＳ２２〜Ｓ２７の処理とを繰り返す。
【０２０１】
この場合、Ｓ２１における暫定段差エッジパラメータの更新は、図７のＳ１での更新処理と同じ手法で行なわれる。
【０２０２】
そして、段差エッジ特定手段１２は、ｎ回目のＳ２７の処理で算出した評価関数Ｅ２の値と、ｎ−１回目のＳ２７の処理で算出した評価関数Ｅ２の値とを比較し、それらの値の差の絶対値が、“０”近傍の所定の設定値よりも小さくなった場合にｎ回目のＳ２２〜Ｓ２７の処理で使用した暫定段差エッジパラメータを、実際の段差エッジ５２の位置及び方向を表す最適段差エッジパラメータとして決定する。
【０２０３】
これにより、評価関数Ｅ２の値を最小化するように最適段差エッジパラメータが探索的に特定されることとなる。例えば、ｎ回目の暫定段差エッジパラメータにより規定される画像上暫定段差エッジラインＬepが図６（ｂ）に示すＬep(n)である場合、このＬep(n)に対応する暫定段差エッジパラメータが最適段差エッジパラメータとして決定される。
【０２０４】
なお、Ｓ２１での暫定段差エッジパラメータの更新処理では、Ｓ１での更新処理に関して補足した如く、種々様々な公知の探索手法を利用できる。また、ｎ回目のＳ２７の処理で算出した評価関数Ｅ２の値が、“０”近傍の所定の設定値よりも小さい場合、あるいは、暫定段差エッジパラメータを更新するＳ２１の処理と、それに続くＳ２２〜Ｓ２７の処理との繰り返し回数ｎが、所定回数に達した場合に、ｎ回目の処理で使用した暫定段差エッジパラメータを、実際の段差エッジ５２の位置及び方向の推定値を表す最適段差エッジパラメータとして決定するようにしてもよい。
【０２０５】
以上が、カメラ視線が上・下方向の場合における段差エッジ特定手段１２の処理である。
【０２０６】
以上説明した本実施形態によれば、カメラ視線が下・上方向である場合と、上・下方向である場合とのいずれの場合でも、第１の撮像画像と第２の撮像画像とから構成されるステレオ画像と、射影変換（平面射影変換）とを利用した探索手法によって、実際の段差エッジ５２の位置及び方向の推定値を表す最適段差エッジパラメータを特定できる。
【０２０７】
この場合、カメラ視線が下・上方向である場合には、段差エッジ特定手段１２は、暫定段差エッジパラメータに応じて第１の撮像画像の画像処理領域ＡＲ１内に領域Ａa1，Ａb1，Ａc1を設定し、それぞれの領域Ａa1，Ａb1，Ａc1の画像の画素値分布と、それぞれの領域Ａa1，Ａb1，Ａc1に対応する平面パラメータ（平面パラメータベクトル↑ｍａ，↑ｍｂ，↑ｍｃ）に応じた射影変換を第２の撮像画像中の対応領域Ａa2，Ａb2，Ａc2の画像に施してなる射影変換画像の画素値分布と誤差、ひいては、前記評価関数Ｅ１の値を最小化するように暫定段差エッジパラメータを更新していくことで、最適段差エッジパラメータを探索的に特定する。
【０２０８】
また、カメラ視線が上・下方向である場合には、段差エッジ特定手段１２は、暫定段差エッジパラメータに応じて第１の撮像画像の画像処理領域ＡＲ２内に領域Ａa3，Ａb3を設定し、それぞれの領域Ａa3，Ａb3の画像の画素値分布と、それぞれの領域Ａa3，Ａb3に対応する平面パラメータ（平面パラメータベクトル↑ｍａ，↑ｍｂ）に応じた射影変換を第２の撮像画像中の対応領域Ａa4，Ａb4の画像に施してなる射影変換画像の画素値分布と誤差、ひいては、前記評価関数Ｅ２の値を最小化するように暫定段差エッジパラメータを更新していくことで、最適段差エッジパラメータを探索的に特定する。
【０２０９】
これにより、カメラ視線が下・上方向である場合及び上・下方向である場合のいずれの場合においても、各撮像画像における段差エッジそのものが、その周辺の部分の画像との区別がつきにくいものとなっている場合であっても、最適段差エッジパラメータを高いロバスト性で安定に特定することができる。
【０２１０】
特に、カメラ視線が下・上方向である場合には、領域Ａc1の上側の境界線としての画像上段差エッジラインＬepとこれと反対側の境界線（下側の境界線）との間の間隔Δｃ、並びに、領域Ａc1の下側の境界線と領域Ａb1の上側の境界線との間の間隔Δｂとを前記した如く設定したことによって、階段５０の上段側の平坦面部５１ａと、下段側の平坦面部５１ｂとの間の蹴込みに相当する部分の画像が評価関数Ｅ１の値に影響を及ぼすのを抑制することができる。
【０２１１】
このため、上段側の平坦面部５１ａと下段側の平坦面部５１ｂとの間に、上段側平面ＰＬａ、起立平面ＰＬｃ、及び下段側平面ＰＬｂから逸脱した部分が存在する場合であっても、実際の段差エッジの位置及び方向を表すものとして信頼性の高い最適段差エッジパラメータを、高いロバスト性で安定に特定することができる。
【０２１２】
次に、本発明の他の態様の実施形態をいくつか説明しておく。
【０２１３】
前記実施形態では、カメラ視線が下・上方向である場合における図７のＳ３の処理で、第１の撮像画像での領域Ａb1の上側の境界線を、領域Ａc1の下側の境界線と所定の間隔Δｂを存して平行に延在するように設定したが、領域Ａb1の上側の境界線を次のように設定してもよい。すなわち、図１２に示すように、起立平面ＰＬｃ（図７のＳ２で設定した起立平面パラメータにより規定される起立平面）と、下段側平面ＰＬｂとの交線を第１の撮像画像に投影してなる直線上の線分（当該直線のうちの画像処理領域ＡＲ１内の部分）を、領域Ａb1の上側の境界線として設定してもよい。
【０２１４】
このようにすることで、前記の実施形態と同様に、階段５０の上段側の平坦面部５１ａと、下段側の平坦面部５１ｂとの間の蹴込みに相当する部分の画像が評価関数Ｅ１の値に影響を及ぼすのを抑制することができる。また、撮像カメラ３Ｒ，３Ｌと階段５０（段差部）との距離によらずに、暫定段差エッジラインＬｅが、実際の段差エッジに合致もしくはほぼ合致する場合に、領域Ａb1の上側の境界線が、下段側の平坦面部５１ｂに存在するようにすることができる。従って、暫定段差エッジラインＬｅと、実際の段差エッジとの一致度合いを、前記誤差関数Ｄｂ１に適切に反映させ、ひいては、前記評価関数Ｅ１の信頼性を適切に確保することができる。
【０２１５】
また、カメラ視線が上・下方向である場合においては、第１の撮像画像における下段側の平坦面部５１ｂの画像のうち、位置及び方向を推定しようとする実際の段差エッジに近接した部分の画像が、第２の撮像画像中には写っていない（上段側の平坦面部５１ａに陰に隠れてしまう）というようなオクルージョンが発生する場合がある。
【０２１６】
そして、このような場合に、前記実施形態の如く、第１の撮像画像おける画像上暫定段差エッジラインＬepが領域Ａa3，Ａb3の境界線となるように領域Ａa3，Ａb3を設定すると、暫定段差エッジラインＬｅが、実際の段差エッジに合致もしくはほぼ合致する状況で、領域Ａb3を下段側平面ＰＬｂの平面パラメータベクトル↑ｍｂに応じて射影変換してなる第２の撮像画像中の領域（領域Ａb3に対応する第２の撮像画像中の領域）に、上段側の平坦面部５１ａの画像が含まれてしまうこととなる。ひいては、前記式（７）により算出される誤差関数Ｄｂ２に、上記の如きオクルージョンに起因する誤差成分がノイズ成分として含まれることとなる。
【０２１７】
従って、上記の如きオクルージョンが発生する状況では、暫定段差エッジラインＬｅが、実際の段差エッジに合致もしくはほぼ合致する場合に、該オクルージョンに起因する誤差成分が、前記式（７）により算出される誤差関数Ｄｂ２に含まれるのを防止するように、前記領域Ａb3を設定することが望ましい。
【０２１８】
カメラ視線が上・下方向である場合における図１１のＳ２２の処理において、このようにオクルージョンの発生を考慮して領域Ａa3，Ａb3を設定する手法の一例を以下に図１３を参照して説明する。なお、図１３では、便宜上、第１の撮像画像における画像上暫定段差エッジラインＬep(k)は、実際の段差エッジの画像に概ね合致しているものとして示している。
【０２１９】
この手法では、段差エッジ特定手段１２は、領域Ａa3を、前記の実施形態と同様に設定する。すなわち、画像上暫定段差エッジラインＬep(k)が、領域Ａa3の上側の境界線とされ、第１の撮像画像内の画像処理領域ＡＲ２のうち、画像上暫定段差エッジラインＬep(k)から下側の領域が、領域Ａa3として設定される。
【０２２０】
一方、段差エッジ特定手段１２は、領域Ａb3を次のように設定する。まず、段差エッジ特定手段１２は、上段側平面ＰＬａの平面パラメータベクトル↑ｍａを前記式（１−１）の↑ｍに代入することで決定した射影変換行列Ｐａにより、第１の撮像画像における画像上暫定段差エッジラインＬep(k)（ｋは、暫定段差エッジパラメータの更新回数を表す）を射影変換する。これにより、図１３に示される如く、第２の撮像画像上の線分Ｌ１が得られる。
【０２２１】
また、段差エッジ特定手段１２は、下段側平面ＰＬｂの平面パラメータベクトル↑ｍｂを前記式（１−１）の↑ｍに代入することで決定した射影変換行列Ｐｂにより、第１の撮像画像における画像上暫定段差エッジラインＬep(k)を射影変換する。これにより、図１３に示される如く、第２の撮像画像上の線分Ｌ２が得られる。
【０２２２】
ここで、オクルージョンが発生している場合には、第２の撮像画像において、線分Ｌ１は、線分Ｌ２よりも上段側の平坦面部５１ａから下段側の平坦面部５１ｂに向う向き（概ね上向き）に変位した位置に存在する。
【０２２３】
そこで、段差エッジ特定手段１２は、第２の撮像画像において、線分Ｌ１が、線分Ｌ２よりも上段側の平坦面部５１ａから下段側の平坦面部５１ｂに向う向き（概ね上向き）に変位した位置に存在するか否かを判断する（以降、この判断処理をオクルージョン発生判断処理という）。そして、このオクルージョン発生判断処理の判断結果が肯定的である場合には、段差エッジ特定手段１２は、領域Ａb3の下側の境界線Ｌ３を、図１３に示す如く画像上暫定段差エッジラインＬep(k)（領域Ａa3の上側の境界線）よりも下段側の平坦面部５１ｂ寄りの位置で、Ｌep(k)と間隔を存してほぼ平行に延在するように設定する。そして、段差エッジ特定手段１２は、画像処理領域ＡＲ２のうち、上記境界線Ｌ３から上側の領域を領域Ａb3として設定する。
【０２２４】
この場合、上記境界線Ｌ３は、より具体的には次のように設定される。すなわち、第１の撮像画像における画像上暫定段差エッジラインＬep(k)を射影変換行列Ｐａにより射影変換した場合に得られた線分Ｌ１を、下段側平面パラメータ↑ｍｂに応じた射影変換行列Ｐｂの逆変換の射影変換行列Ｐｂ-1によって射影変換することで得られる線が、領域Ａb3の下側の境界線Ｌ３として設定される。
【０２２５】
また、オクルージョン発生判断処理の判断結果が否定的である場合には、段差エッジ特定手段１２は、画像上暫定段差エッジラインＬep(k)（領域Ａa3の上側の境界線）を、領域Ａb3の下側の境界線Ｌ３とし、画像処理領域ＡＲ２のうち、該境界線Ｌ３から上側の領域を領域Ａb3として設定する。
【０２２６】
以上が、オクルージョンが発生している場合の領域Ａa3，Ａb3の設定手法の一例である。
【０２２７】
カメラ視線が上・下方向である場合における図１１のＳ２２の処理において、上記の如く領域Ａa3，Ａb3を設定することによって、オクルージョンに起因する誤差成分が、前記式（７）により算出される誤差関数Ｄｂ２に含まれるのを防止することができる。このため、前記式（８）により算出される評価関数Ｅ２が、暫定段差エッジＬｅと、実際の段差エッジとの一致度合いを表すものとしての信頼性をより一層高めることができる。その結果、実際の段差エッジの位置及び方向を表すものとして最終的に特定される最適段差エッジパラメータの信頼性をより一層高めることができる。
【０２２８】
補足すると、以上説明した領域Ａa3，Ａb3の設定手法の例では、第１の撮像画像における画像上暫定段差エッジラインＬep(k)が、本発明における第１暫定段差エッジ線に相当し、第２の撮像画像上の線分Ｌ１，Ｌ２がそれぞれ、本発明における第２Ａ暫定段差エッジ線、第２Ｂ暫定段差エッジ線に相当する。
【０２２９】
また、前記実施形態では、暫定段差エッジパラメータを、前記評価関数Ｅ１又はＥ２の値に応じて初期値から逐次更新していく処理によって、最適段差エッジパラメータを特定するようにした。但し、最適段差エッジパラメータの特定を以下に説明するように行なうようにしてもよい。
【０２３０】
例えばカメラ視線が下・上方向である場合について代表的に説明すると、前記段差エッジ特定手段１２により最適段差エッジパラメータを特定する場合に、まず、実際の段差エッジの位置及び方向の推定値を表すものとして採用し得る可能性のある暫定段差エッジパラメータの複数の候補を決定しておく。
【０２３１】
この複数の暫定段差エッジパラメータの候補は、例えば次のように決定される。すなわち、各候補に対応する画像上暫定段差エッジラインＬepが、第１の撮像画像内で前記平坦面部認識手段１１によって抽出された上段側の平坦面部５１ａに対応する平坦面部投影領域Ａr1の下側の境界線（下段側の平坦面部５１ｂに対応する平坦面部投影領域Ａr2寄りの境界線）の近辺に存在し、且つ、それらの各候補に対応する画像上暫定段差エッジラインＬepの位置（具体的には、第１の撮像画像の画像処理領域ＡＲ１内での画像上暫定段差エッジラインＬepの中央点の位置（第１の撮像画像の縦方向での位置）と、方向（具体的には、第１の撮像画像の横方向に対する傾斜角度）との両方又は一方が各候補毎に互いに異なるものとなるように決定される。
【０２３２】
この場合、各候補に対応する画像上暫定段差エッジラインＬepの位置の種類数と該画像上暫定段差エッジラインＬepの方向の種類数とは、それぞれ、所定数ｍ１、ｍ２（ｍ１，ｍ２は２以上の整数）とされ、トータルｍ１×ｍ２個の暫定段差エッジパラメータの候補が決定される。なお、各候補に対応する画像上暫定段差エッジラインＬepのｍ１個の位置は、例えば、上段側の平坦面部５１ａに対応する平坦面部投影領域Ａr1の下側の境界線の近辺の所定範囲内で第１の撮像画像の縦方向に微小量づつ、異なる位置となるように決定される。また、各候補に対応する画像上暫定段差エッジラインＬepのｍ２個の方向は、例えば、第１の撮像画像の横方向に対する傾斜角度（鋭角側の傾斜角度）が０°近辺の所定範囲内で微小量づつ、異なる角度となるように決定される。
【０２３３】
そして、上記のように決定した暫定段差エッジパラメータの各候補毎に、前記した図７のＳ２〜Ｓ１０と同じ処理を実行することによって評価関数Ｅ１の値を算出し、暫定段差エッジパラメータの候補のうち、算出した評価関数Ｅ１の値が最小となる候補を、最適段差エッジパラメータとして特定する。
【０２３４】
カメラ視線が上・下方向である場合においても、上記と同様に、暫定段差エッジパラメータの複数の候補の中から、最適段差エッジパラメータを特定することができる。この場合は、暫定段差エッジパラメータの複数の候補の設定の仕方は、カメラ視線が下・上方向である場合と同じよい。そして、各候補毎の評価関数Ｅ２は、前記した図１１のＳ２２〜Ｓ２７と同じ処理を実行することによって算出するようにすればよい。
【０２３５】
また、以上説明した実施形態では、カメラ視線が下・上方向である場合に、評価関数Ｅ１を、３つの誤差関数Ｄａ１，Ｄｂ１，Ｄｃ１を線形結合することによって構成したが、誤差関数Ｄｂ１を省略し、誤差関数Ｄａ１，Ｄｃ１の２つの誤差関数を線形結合することによって評価関数Ｅ１を構成するようにしてもよい。この場合には、第１の撮像画像における領域Ａb1と、これに対応する第２の撮像画像の領域Ａb2の設定は不要である。
【０２３６】
また、前記実施形態では、カメラ視線が下・上方向である場合に、上段側平面ＰＬａに対する前記起立平面ＰＬｃの傾斜角度を既定値とすることで、段差エッジ特定手段１２の演算処理負負荷が軽減されることとなるが、段差エッジパラメータに加えて、該傾斜角度も暫定的に仮決定するようにして、前記評価関数Ｅ１の値を最小化するように、段差エッジパラメータと該傾斜角度との両方を探索的に決定するようにしてもよい。
【０２３７】
また、前記実施形態では、カメラ３Ｒの撮像画像を第１の撮像画像として使用する場合を例にとって説明したが、カメラ３Ｌの撮像画像を第１の撮像画像として使用してもよいことはもちろんである。
【０２３８】
また、前記実施形態では、段差部としての階段５０の段差エッジの位置及び方向を推定する場合を主要例として説明したが、階段５０以外の段差部、例えば、床に設置される踏み台等の設置物により構成される段差部の段差エッジであっても、その位置及び方向を前記実施形態と同様に推定することができる。
【０２３９】
また、前記実施形態では、カメラ３Ｒ，３Ｌをロボット１に搭載した場合を例にとって説明したが、カメラ３Ｒ，３Ｌは、階段等の段差部を移動可能なロボット１以外の移動体に搭載したものでもあってもよい。
【符号の説明】
【０２４０】
３Ｒ，３Ｌ…カメラ（撮像手段）、１２…段差エッジ特定手段、５０…階段（段差部）、５１ａ…上段側の平坦面部、５１ｂ…下段側の平坦面部、５２…段差エッジ、Ｓ３，Ｓ２２…評価関数算出用領域設定手段、Ａa1…第１の領域、Ａb1…第２の領域、Ａc1…第３の領域、Ａa3…第４の領域、Ａb3…第５の領域、Ｅ１，Ｅ２…評価関数、Ｄａ１…第１誤差関数、Ｄｂ１…第２誤差関数、Ｄｃ１…第３誤差関数、Ｄａ２…第４誤差関数、Ｄｂ２…第５誤差関数。
【技術分野】
【０００１】
本発明は、撮像画像を用いて階段のエッジ等の段差エッジの位置及び方向を推定する装置に関する。
【背景技術】
【０００２】
従来、例えば自律的に移動可能なロボットに階段の昇降動作を行なわせる場合に、該ロボットに階段を認識させる技術として、例えば特許文献１に見られる技術が提案されている。
【０００３】
この技術では、ロボットに搭載したステレオビジョンシステムによってロボットの外界の距離データを取得し、この距離データを基に、階段の踏面に相当する平面部を検出する。そして、検出した平面部を包含する多角形をSklanskyのアルゴリズムや、Melkmanのアルゴリズムによって求め、その求めた多角形から、階段のフロントエッジ、バックエッジ、幅、長さを特定する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】国際公開公報ＷＯ2005/087452
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ところで、２足移動ロボット等の脚式移動ロボットに階段等の段差部の昇降動作を行なわせる場合、ロボットの脚の足平部を、段差部の上段側又は下段側の平坦面部（以下、踏面ということがある）の先端部のエッジ（特許文献１で言うところのフロントエッジ）に載せて、該足平部の一部を段差部の踏面からはみ出させることが必要もしくは好ましい場合がある。
【０００６】
例えば、階段の踏面の奥行き寸法が、ロボットの足平部の大きさに比して狭い場合には、該足平部の全体を踏面に載せることが不可能な場合がある。あるいは、ロボットの足平部の全体を踏面に載せることが可能であっても、該足平部を振り出すときに、該足平部が階段の蹴り込み等と干渉するのを避けるために、ロボットの足平部の一部を踏面からはみ出させることが好ましい場合もある。
【０００７】
そして、このような場合には、ロボットの足平部の目標とする着地位置を適切に決定するために、階段等の段差部の先端部のエッジの位置及びその方向を精度よく認識する必要がある。
【０００８】
しかるに、前記特許文献１に見られる如き従来の手法では、階段などの段差部の踏面の先端部のエッジを検出することができない場合が多々ある。
【０００９】
すなわち、ステレオビジョンシステムによって得られるステレオ画像から認識される距離データは、撮像画像のテクスチャ等の影響を受けやすいので、段差部の踏面の先端部のエッジ付近での信頼性の高い距離データを得ることができない場合も多々ある。そして、このような場合には、段差部の踏面の先端部のエッジ付近は、平面部として認識できないか、もしくは、平面部から除外されることとなる。その結果、前記多角形は、段差部の踏面の先端部のエッジと異なる境界の形状を有するものとなり、該多角形から認識されるフロントエッジは、段差部の踏面の先端部の実際のエッジとの誤差が大きなものとなりやすい。
【００１０】
従って、前記特許文献１に見られる如き従来の手法では、階段などの段差部の踏面の先端部のエッジを検出することができない場合が多々ある。
【００１１】
本発明は、かかる背景に鑑みてなされたものであり、撮像手段により取得されるステレオ画像を利用して、階段等の段差部の平坦面部の先端部のエッジの位置及び方向を精度良く推定することを可能とする装置を提供することを目的とする。
【課題を解決するための手段】
【００１２】
ここで、本発明における用語について補足しておく。本発明においては、「段差部」は、上段側の平坦面部とその一段下側に存在する下段側の平坦面部との組が一組もしくは複数組含まれるものである。この「段差部」は、階段を含むものであるが、階段だけを意味するものではない。例えば、床面に設置される踏み台もしくは任意の設置物により構成される段差部も含まれる。
【００１３】
また、段差部の平坦面部の「先端側エッジ」又は「段差エッジ」というのは、段差部をその下段側が手前側、上段側が奥側となる視線で見た場合において、各段の平坦面部（階段で言うところの踏面）の外形の境界線のうち、手前側で横方向（水平方向もしくはそれに近い方向）に直線状に延在する境界線を意味する。
【００１４】
あるいは、換言すれば、段差部の平坦面部の「先端側エッジ」又は「段差エッジ」というのは、段差部をその上段側が手前側、下段側が奥側となる視線で見た場合において、各段の平坦面部の外形の境界線のうち、奥側で横方向に直線状に延在する境界線を意味する。
【００１５】
以上を前提として、本発明を以下に説明する。
【００１６】
本発明は、上記目的を達成するために、第１の撮像画像及び第２の撮像画像から構成されるステレオ画像を撮像するための撮像手段を用い、下段側の平坦面部と、該下段側の平坦面部の一段上側の上段側の平坦面部との組を少なくとも一組有する段差部を、その下段側が手前側、上段側が奥側となる視線で前記撮像手段により撮像し、その撮像により得られた該段差部のステレオ画像を基に、前記上段側の平坦面部の先端側エッジである段差エッジの３次元空間中の位置及び方向を推定する段差エッジ推定装置であって、
前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値を、暫定的に仮決定した段差エッジパラメータと、前記上段側の平坦面部を包含する上段側平面の位置及び姿勢を表すものとして事前に特定された上段側平面パラメータと、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジを含むと共に前記上段側平面に対して起立した姿勢となる起立平面の位置及び姿勢を表すものとして、前記仮決定した段差エッジパラメータ及び前記上段側平面パラメータに応じて設定した起立平面パラメータとを用いて算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する段差エッジ特定手段を備え、
前記評価関数は、
前記第１の撮像画像中に設定した第１の領域における画像の画素値分布と、前記第２の撮像画像中の前記第１の領域に対応する領域における画像を前記起立平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第１誤差関数と、
前記第１の撮像画像中に設定した第２の領域における画像の画素値分布と、前記第２の撮像画像中の前記第２の領域に対応する領域における画像を前記上段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第２誤差関数とを線形結合して構成された評価関数であり、
前記段差エッジ特定手段は、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第１の撮像画像中において前記第１の領域及び第２の領域の境界線となると共に、前記第１の領域が、前記第１の撮像画像中において前記上段側の平坦面部とその一段下側の下段側の平坦面部との間の部分である上下段間部分と前記上段側の平坦面部とのうちの上下段間部分寄りの領域、前記第２の領域が前記上下段間部分と前記上段側の平坦面部とのうちの上段側の平坦面部寄りの領域となるように該第１の領域及び第２の領域を設定する評価関数算出用領域設定手段を含むことを特徴とする（第１発明）。
【００１７】
ここで、一般に、前記段差部の上段側の平坦面部の先端部には、階段における段鼻の先端面の如く、上段側の平坦面部からこれに対して起立した姿勢で垂下するほぼ平坦な起立面が存在し、この起立面を包含する起立平面と上段側の平坦面部を包含する上段側平面との交線が前記段差エッジに相当するものである。そして、前記段差部をその下段側が手前側、上段側が奥側となる視線で前記撮像手段により撮像した場合には、該起立面が第１の撮像画像及び第２の撮像画像に投影されることとなる。
【００１８】
また、各撮像画像における上段側の平坦面部や下段側の平坦面部は比較的広い面積を有することから、それらの平坦面部の撮像手段に対する位置及び姿勢は、距離センサや前記ステレオ画像等を利用した公知の手法によって、比較的容易に特定しておくことができる。
【００１９】
さらに、前記段差エッジの位置及び方向を、暫定的に、ある適当な位置及び方向に仮決定し、その仮決定した位置及び方向を有する段差エッジが上段側平面との交線となるように起立平面を設定した場合に、その起立平面が実際の段差部における前記起立面を包含する平面であるか否かは、前記第１の撮像画像及び第２の撮像画像から構成されるステレオ画像と射影変換（平面射影変換）とを利用して評価することが可能である。
【００２０】
前記第１発明は、これらの事項を考慮して構築されたものである。
【００２１】
この第１発明によれば、前記段差エッジ特定手段は、前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値の大きさを最小化するように、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する。
【００２２】
この場合、上記評価関数は、前記第１誤差関数と第２誤差関数とを線形結合して構成される。そして、この第１誤差関数は、前記第１の撮像画像中に設定した第１の領域における画像の画素値分布と、前記第２の撮像画像中の前記第１の領域に対応する領域における画像を、暫定的に仮決定した段差エッジパラメータと事前に特定された上段側平面パラメータとに応じて設定した前記起立平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す関数である。
【００２３】
また、第２誤差関数は、前記第１の撮像画像中に設定した第２の領域における画像の画素値分布と、前記第２の撮像画像中の前記第２の領域に対応する領域における画像を前記上段側平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す関数である。
【００２４】
さらに、前記第１の領域と第２の領域とは、前記した如く、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジを前記第１の撮像画像中における両領域の境界線として設定される領域である。
【００２５】
このため、前記評価関数の値は、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジの位置及び方向の、実際の段差エッジの位置及び方向に対する誤差の度合いを表すものとなり、暫定的に仮決定した段差エッジパラメータにより規定される段差エッジの位置及び方向が、実際の段差エッジの位置及び方向に合致していれば、前記評価関数の値は、最小となるものである。
【００２６】
従って、暫定的に仮決定した段差エッジパラメータに対して前記評価関数を算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、前記最適段差エッジパラメータを特定することによって、該最適段差エッジパラメータを、実際の段差エッジの位置及び姿勢の推定値を表すものとして得られることとなる。
【００２７】
なお、前記評価関数の値の大きさを最小化するように、前記最適段差エッジパラメータを特定するということは、評価関数の値が最小値、もしくは、その近似値（当該最小値との差が所要の許容範囲（段差エッジの位置及び方向の推定精度を目標とする精度に確保するために要求される許容範囲）内に収まる値）となると見なし得る段差エッジパラメータを、最適段差エッジパラメータとして特定することを意味する。従って、特定した最適段差エッジパラメータに対して算出される前記評価関数の値が厳密に該評価関数の最小値である必要はない。
【００２８】
かかる第１発明によれば、前記段差エッジが、前記上段側平面と起立平面との交線となることを前提とすることによって、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジの位置及び方向の、実際の段差エッジの位置及び方向に対する誤差の度合いを、前記した如く構成した評価関数の値によって適正に評価することができる。このため、撮像手段により取得される第１の撮像画像及び第２の撮像画像から構成されるステレオ画像を利用して、階段等の段差部の平坦面部の先端部のエッジの位置及び方向を精度良く推定することが可能となる。
【００２９】
なお、第１誤差関数の代表的な例としては、前記第１の撮像画像中の第１の領域における画像の各画素値（輝度値など）と、前記第２の撮像画像中の前記第１の領域に対応する領域における画像の射影変換画像の各画素値との偏差の絶対値又は二乗値の積算値、あるいは、該絶対値又は二乗値の平均値が挙げられる。第２誤差関数についても同様である。
【００３０】
前記第１発明では、前記段差エッジ特定手段は、前記上段側平面又は水平面に対する前記起立平面の傾斜角度を、事前に設定された所定の角度に規定して前記起立平面パラメータを設定することが好ましい（第２発明）。
【００３１】
この第２発明によれば、前記上段側平面又は水平面に対する前記起立平面の傾斜角度が既定値とされることとなるので、暫定的に仮決定した段差エッジパラメータと、前記上段側平面パラメータとから前記起立平面パラメータを設定できる。このため、未知数が、段差エッジパラメータだけとなり、前記最適段差エッジパラメータの探索的な特定処理が容易になる。
【００３２】
但し、第１発明においては、前記起立平面の傾斜角度と、段差エッジパラメータとの組を未知数として、該傾斜角度と最適段差エッジパラメータとを特定するようにすることも可能である。
【００３３】
また、第１発明又は第２発明においては、前記評価関数算出用領域設定手段は、前記第１の撮像画像中において、前記第１の領域の前記第２の領域と反対側の境界線が、前記暫定的な段差エッジと所定の間隔を存して平行に延在するように該第１の領域を設定する手段を含んでおり、該所定の間隔は、前記暫定的な段差エッジが実際の段差エッジに一致する場合に、前記第１の領域の前記第２の領域と反対側の境界線が、前記上下段間部分において前記上段側の平坦面部とその一段下側の下段側の平坦面部とのうちの上段側の平坦面部寄りの境界線となるように設定された間隔であることが好ましい（第３発明）。
【００３４】
すなわち、一般的な階段等の段差部において、上段側の平坦面部の先端部で、該平坦面部からこれに対して起立した姿勢で垂下する起立面は、多くの場合、下段側の平坦面部まで同一平面上で連なっているわけではなく、上段側の平坦面部寄りの箇所で、高さ方向に比較的小さな幅（上段側の平坦面部を構成する板部材の厚さ分に相当する幅）を有する面となっている場合が多い。そして、上段側の平坦面部と下段側の平坦面部との間の箇所（階段の蹴込みに相当する箇所）のうちの、下段側の平坦面部寄りの箇所には、起立面が存在しないか、又は、上段側の平坦面部に連なる起立面よりも奥側に存在する面となっている場合が多い。
【００３５】
そこで、第３発明では、上記の如く、第１の領域を設定するようにした。これにより、特定の種類に限られない一般的な形状の種々様々な段差部に対して、前記暫定的な段差エッジが実際の段差エッジに一致する場合における第１の領域の全体又は大部分が、段差部における前記起立平面上に存在する部分の画像となるように、前記第１の領域を設定できることとなる。この結果、一般的な形状の種々様々な段差部に対して、前記評価関数の信頼性（暫定的な段差エッジの位置及び方向の誤差を表すものとしての信頼性）を高め、ひいては、実際の段差エッジの位置及び方向の推定の信頼性を高めることができると共に、その推定処理の汎用性を高めることができる。
【００３６】
前記第１〜第３の発明において、前記最適段差エッジパラメータを特定するための代表的な手法としては、次のような２つの手法を用いることができる。その第１の手法では、前記段差エッジ特定手段は、暫定的に仮決定する前記段差エッジパラメータの候補としてあらかじめ定められた複数の段差エッジパラメータ候補のうち、当該候補に対応して算出される前記評価関数の値が最小となる段差エッジパラメータ候補を前記最適段差エッジパラメータとして特定するように構成される。
【００３７】
また、第２の手法では、前記段差エッジ特定手段は、暫定的に仮決定する前記段差エッジパラメータを、その初期値から逐次更新していくことによって、前記最適段差エッジパラメータを特定するように構成される。なお、暫定的に仮決定する前記段差エッジパラメータを、その初期値から逐次更新していくということは、より詳しくは、段差エッジパタメータを暫定的に仮決定し、少なくともその仮決定した段差エッジパラメータに対応して算出される評価関数の値に応じて、新たな暫定的な段差エッジパラメータを仮決定するという処理を繰り返すことを意味する。
【００３８】
前記第１〜第３の発明においては、上記第１の手法及び第２の手法のいずれの手法を採用することもできる。そして、特に上記第２の手法を採用する場合には、前記初期値は、次のように設定することが好適である。すなわち、前記第１〜第３の発明において、前記段差エッジ特定手段が、暫定的に仮決定する前記段差エッジパラメータを、その初期値から逐次更新していくことによって、前記最適段差エッジパラメータを特定するように構成される場合には、該段差エッジ特定手段は、前記段差エッジパラメータの初期値により規定される暫定的な段差エッジが、前記上段側の平坦面部のうち、平面であることが事前に特定されている領域を通るように該初期値を設定する手段を含むことが好ましい（第４発明）。
【００３９】
この第４発明によれば、暫定的に仮決定する前記段差エッジパラメータの初期値を、上記の如く設定することによって、前記最適段差エッジパラメータを特定するための暫定的な段差エッジパラメータの更新処理の安定性（ロバスト性）を高め、暫定的な段差エッジパラメータが発散してしまうように更新されてしまうのを防止することができる。
【００４０】
この第４発明では、前記撮像手段が、前記段差部を移動する移動体に、該移動体の進行方向前方を撮像するように搭載されている場合には、前記段差エッジパラメータの初期値を設定する手段は、該初期値により規定される暫定的な段差エッジの方向が前記移動体の進行方向に対して交差する方向になるように該初期値を設定することが好ましい（第５発明）。
【００４１】
この第５発明よれば、暫定的に仮設定する段差エッジパラメータの初期値により規定される段差エッジの方向が、実際の方向に近い方向となるので、段差エッジパラメータの更新処理の安定性（ロバスト性）を効果的に高めることができる。
【００４２】
また、前記第１〜第５発明では、前記評価関数は、前記第１の撮像画像中に設定した第３の領域における画像の画素値分布と、前記第２の撮像画像中の前記第３の領域に対応する領域における画像を、前記上段側の平坦面部の一段下側の下段側の平坦面部を包含する下段側平面の位置及び姿勢を表すものとして事前に特定された下段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第３誤差関数を、前記第１誤差関数と第２誤差関数とにさらに線形結合して構成された評価関数であり、
前記評価関数算出用領域設定手段は、前記第３の領域が、前記第１の撮像画像中において前記第１の領域よりも前記下段側の平坦面部寄りの位置で該下段側の平坦面部の少なくとも一部分を含むと共に、該第３の領域の前記第１の領域側の境界線の該第１の撮像画像中での位置及び方向を、該第１の撮像画像中での前記暫定的な段差エッジの位置及び方向に応じて規定するように該第３の領域を設定する手段を含むようにしてもよい（第６発明）。
【００４３】
この第６発明によれば、前記評価関数は、前記第１誤差関数及び第２誤差関数に、さらに、前記第３誤差関数を加えた３つの誤差関数を線形結合することによって構成される。そして、この場合、前記第３誤差関数を算出するための第１の撮像画像中の第３の領域は、上記の如く設定される。
【００４４】
このため、第１及び第２の撮像画像における段差部の上段側の平坦面部及び段差エッジの近辺の撮像画像に加えて、下段側の平坦面部の近辺の撮像画像の画素値データが、暫定的に仮決定する段差エッジパラメータに依存性を有するように前記第３誤差関数を介して評価関数に反映されることとなる。この結果、この評価関数の値を最小化するように決定した最適段差エッジパラメータの信頼性と、該最適段差エッジパラメータの探索的な特定処理のロバスト性とを高めることができる。
【００４５】
この第６発明では、前記第３の領域を設定する手段は、該第３の領域の前記第１の領域側の境界線が、前記暫定的な段差エッジと平行に延在するように該第３の領域を設定することが好ましい（第７発明）。
【００４６】
この第７発明によれば、暫定的に仮決定する段差エッジパラメータを前記第３の領域に適切に反映させ、前記第３の誤差関数が、暫定的に仮決定する段差エッジパラメータに依存して変化するようにすることができる。ひいては、前記評価関数に、下段側の平坦面部の近辺の撮像画像の画素値データを適切に反映させることができる。
【００４７】
上記第６発明又は第７発明では、前記第３の領域を設定する手段は、前記第１の撮像画像において第１の領域と第３の領域との間に所定の間隔を存するように該第３の領域を設定するものであり、該所定の間隔は、前記暫定的な段差エッジが実際の段差エッジに一致する場合に、前記第３の領域の前記第１の領域側の境界線が、前記第１の撮像画像において前記下段側の平坦面部内に存在するように設定された間隔であることが好ましい（第８発明）。
【００４８】
これによれば、前記第１の領域と第３の領域との間に、上記所定の間隔を設定しておくことで、第１の領域と第３の領域との間の間隔内の画像は前記第３誤差関数、ひいては、前記評価関数の値に影響を及ぼさないものとなる。そして、この場合、上記所定の間隔は、前記暫定的な段差エッジが実際の段差エッジに一致する場合に、前記第３の領域の前記第１の領域側の境界線が、前記第１の撮像画像において前記下段側の平坦面部内に存在するように設定された間隔、すなわち、前記暫定的な段差エッジが実際の段差エッジに一致する場合に、第３領域内に前記上下段間部分の画像が含まれないように設定された間隔である。
【００４９】
このため、種々様々の形状の段差部に対して、上段側の平坦面部の先端部の前記起立面と下段側の平坦面部とのいずれとも異なる部分の画像が、前記第１誤差関数又は第３誤差関数の値、ひいては、前記評価関数の値に影響を及ぼすのを極力抑制することができる。その結果、一般的な形状の種々様々な段差部に対して、前記評価関数の信頼性（暫定的な段差エッジの位置及び方向の誤差を表すものとしての信頼性）を高め、ひいては、実際の段差エッジの位置及び方向の推定の信頼性を高めることができると共に、その推定処理の汎用性を高めることができる。
【００５０】
また、前記第６発明では、前記第３の領域を設定する手段は、次のような形態で前記第３の領域の前記第１の領域側の境界線を設定してもよい。
【００５１】
すなわち、前記第６発明において、前記第３の領域を設定する手段は、前記第３の領域の前記第１の領域側の境界線が、前記仮決定した段差エッジパラメータ及び前記上段側平面パラメータに応じて設定された前記起立平面パラメータに対応する前記起立平面と前記下段側平面との交線を前記第１の撮像画像に投影した直線上の線分になるように、該第３の領域の第１の領域側の境界線を設定するようにしてもよい（第９発明）。
【００５２】
この第９発明によれば、暫定的に仮決定する段差エッジパラメータを前記第３の領域に適切に反映させ、前記第３の誤差関数が、暫定的に仮決定する段差エッジパラメータに依存して変化するようにすることができる。ひいては、第６発明と同様に、前記評価関数に、下段側の平坦面部の近辺の撮像画像の画素値データを適切に反映させることができる。
【００５３】
さらに、種々様々の形状の段差部に対して、上段側の平坦面部の先端部の前記起立面と下段側の平坦面部とのいずれとも異なる部分の画像が、前記第１誤差関数又は第３誤差関数の値、ひいては、前記評価関数の値に影響を及ぼすのを極力抑制することができる。その結果、第７発明と同様に、一般的な形状の種々様々な段差部に対して、前記評価関数の信頼性（暫定的な段差エッジの位置及び方向の誤差を表すものとしての信頼性）を高め、ひいては、実際の段差エッジの位置及び方向の推定の信頼性を高めることができると共に、その推定処理の汎用性を高めることができる。
【００５４】
前記段差部をその下段側が手前側、上段側が奥側となる視線で前記撮像手段により撮像した場合には、前記第１〜第９発明によって段差エッジの位置及び方向を推定することができる。一方、前記段差部をその上段側が手前側、下段側が奥側となる視線で前記撮像手段により撮像した場合には、階段の段鼻の先端面の如き段差部の起立面は、前記第１の撮像画像及び第２の撮像画像には写らない。この場合には、以下に説明する第１０発明によって、段差エッジの位置及び方向を推定することができる。
【００５５】
すなわち、第１０発明の段差エッジ推定装置は、第１の撮像画像及び第２の撮像画像から構成されるステレオ画像を撮像するための撮像手段を用い、下段側の平坦面部と、該下段側の平坦面部の一段上側の上段側の平坦面部との組を少なくとも一組有する段差部を、その上段側が手前側、下段側を奥側となる視線で前記撮像手段により撮像し、その撮像により得られた該段差部のステレオ画像を基に、前記上段側の平坦面部の先端側エッジである段差エッジの３次元空間中の位置及び方向を推定する段差エッジ推定装置であって、
前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値を、暫定的に仮決定した段差エッジパラメータと、前記上段側の平坦面部を包含する上段側平面の位置及び姿勢を表すものとして事前に特定された上段側平面パラメータと、前記上段側の平坦面部の一段下側の下段側平坦面部を包含する下段側平面の位置及び姿勢を表すものとして事前に特定された下段側平面パラメータとを用いて算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する段差エッジ特定手段を備え、
前記評価関数は、
前記第１の撮像画像中に設定した第４の領域における画像の画素値分布と、前記第２の撮像画像中の前記第４の領域に対応する領域における画像を前記上段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第４誤差関数と、
前記第１の撮像画像中に設定した第５の領域における画像の画素値分布と、前記第２の撮像画像中の前記第５の領域に対応する領域における画像を前記下段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第５誤差関数とを線形結合して構成された評価関数であり、
前記段差エッジ特定手段は、前記第１の撮像画像中において、前記第４の領域が、前記上段側の平坦面部と前記下段側の平坦面部とのうちの上段側の平坦面部寄りの領域、前記第５の領域が前記上段側の平坦面部と前記下段側の平坦面部とのうちの下段側の平坦面部寄りの領域となると共に、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第１の撮像画像中において前記第４の領域の前記第５の領域側の境界線となり、且つ、前記第５の領域の前記第４の領域側の境界線が、前記第４の領域の前記第５の領域側の境界線に一致するか、又は、前記第４の領域の前記第５の領域側の境界線と間隔を存して当該第５の領域側の境界線と平行に延在するように該第４の領域及び第５の領域を設定する評価関数算出用領域設定手段を含むことを特徴とする（第９発明）。
【００５６】
ここで、前記段差部をその上段側が手前側、下段側が奥側となる視線で前記撮像手段により撮像した場合には、前記第１発明に関して説明した上段側平坦面部の先端部の起立面が第１の撮像画像及び第２の撮像画像に投影されず、それらの第１の撮像画像及び第２の撮像画像中では、段差エッジは、上段側の平坦面部と下段側の平坦面部との境界線として写ることとなる。上記第１０発明は、このことを考慮して構築されたものである。
【００５７】
この第１０発明によれば、前記段差エッジ特定手段は、前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値の大きさを最小化するように、前記暫定的に仮決定した段差エッジパラメータを修正することによって、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する。
【００５８】
この場合、第１０発明における上記評価関数は、前記第４誤差関数と第５誤差関数とを線形結合して構成される。そして、この第４誤差関数は、前記第１の撮像画像中に設定した第４の領域における画像の画素値分布と、前記第２の撮像画像中の前記第４の領域に対応する領域における画像を、前記上段側平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す関数である。
【００５９】
また、第５誤差関数は、前記第１の撮像画像中に設定した第５の領域における画像の画素値分布と、前記第２の撮像画像中の前記第５の領域に対応する領域における画像を、前記下段側平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す関数である。
【００６０】
さらに、前記第４の領域と第５の領域とは、前記した如く、前記第１の撮像画像中において、前記第４の領域が、前記上段側の平坦面部と前記下段側の平坦面部とのうちの上段側の平坦面部寄りの領域、前記第５の領域が前記上段側の平坦面部と前記下段側の平坦面部とのうちの下段側の平坦面部寄りの領域となると共に、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第１の撮像画像中において前記第４の領域の前記第５の領域側の境界線となり、且つ、前記第５の領域の前記第４の領域側の境界線が、前記第４の領域の前記第５の領域側の境界線に一致するか、又は、前記第４の領域の前記第５の領域側の境界線と間隔を存して当該第５の領域側の境界線と平行に延在するように設定される領域である。
【００６１】
このため、第１０発明における前記評価関数の値は、第１発明における評価関数と同様に、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジの位置及び方向の、実際の段差エッジの位置及び方向に対する誤差の度合いを表すものとなり、暫定的に仮決定した段差エッジパラメータにより規定される段差エッジの位置及び方向が、実際の段差エッジの位置及び方向に合致していれば、前記評価関数の値は、最小となるものである。
【００６２】
従って、暫定的に仮決定した段差エッジパラメータに対して前記評価関数を算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、前記暫定的に仮決定した段差エッジパラメータを修正することによって、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定できることとなる。
【００６３】
よって、第１０発明によれば、前記段差エッジが、第１及び第２の撮像画像中において、上段側の平坦面部と下段側の平坦面部との境界線として写ることを前提とすることによって、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジの位置及び方向の、実際の段差エッジの位置及び方向に対する誤差の度合いを、前記した如く構成した評価関数の値によって適正に評価することができる。このため、撮像手段により取得される第１の撮像画像及び第２の撮像画像から構成されるステレオ画像を利用して、階段等の段差部の平坦面部の先端部のエッジの位置及び方向を精度良く推定することが可能となる。
【００６４】
なお、第４誤差関数の代表的な例としては、前記第１の撮像画像中の第４の領域における画像の各画素値（輝度値など）と、前記第２の撮像画像中の前記第４の領域に対応する領域における画像の射影変換画像の各画素値との偏差の絶対値又は二乗値の積算値、あるいは、該絶対値又は二乗値の平均値が挙げられる。第５誤差関数についても同様である。
【００６５】
また、第１０発明において、前記最適段差エッジパラメータを特定するための代表的な手法としては、前記第１〜第３発明に関して説明した第１の手法又は第２の手法と同じ手法を用いることができる。
【００６６】
上記第１０発明において、所謂、オクルージョンによって、第１の撮像画像に写っている下段側の平坦面部のうちの一部、例えば、下段側の平坦面部のうちの、上段側の平坦面部の段差エッジに近い部分が第２の撮像画像には写っていないという場合がある。
【００６７】
そして、このようなオクルージョンが発生している場合には、前記第１の撮像画像において前記第５の領域の前記第４の領域側の境界線が、前記第４の領域の前記第５の領域側の境界線に一致するようにした場合（前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第１の撮像画像中において前記第４の領域と第５の領域との間の境界線となる場合）には、暫定的な段差エッジが実際の段差エッジに一致していても、前記第５誤差関数に上記オクルージョンに起因する誤差成分が含まれることとなる。
【００６８】
従って、上記の如きオクルージョンが発生する状況では、前記第５の領域の前記第４の領域側の境界線が、前記第４の領域の前記第５の領域側の境界線と間隔を存して当該第５の領域側の境界線と平行に延在するように第５の領域を設定して、第５の領域に、第２の撮像画像中に写らないような部分が含まれないようにすることが好ましい。
【００６９】
そこで、第１０発明では、前記評価関数算出用領域設定手段は、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジを前記第１の撮像画像に投影してなる線である第１暫定段差エッジ線を、前記上段側平面パラメータに応じて第１の撮像画像側から第２の撮像画像側に射影変換した場合に得られる第２Ａ暫定段差エッジ線と、前記第１暫定段差エッジ線を、前記下段側平面パラメータに応じて第１の撮像画像側から第２の撮像画像側に射影変換した場合に得られる第２Ｂ暫定段差エッジ線との第２の撮像画像中での位置関係を比較し、該２の撮像画像での第２Ａ暫定段差エッジ線の位置が、前記第２Ｂ暫定段差エッジ線よりも、前記上段側の平坦面部から下段側の平坦面部に向う向きに変位した位置であるか否かを判断する手段と、該判断結果が肯定的である場合には、前記第５の領域の前記第４の領域側の境界線が、前記第２Ａ暫定段差エッジ線を、前記下段側平面パラメータに応じて第２の撮像画像側から第１の撮像画像側に射影変換した場合に得られる線上に存在するように前記第５の領域を設定し、前記判断結果が否定的である場合には、前記第５の領域の前記第４の領域側の境界線が、前記第１暫定段差エッジ線上に存在するように前記第５の領域を設定する手段とを備えることが好ましい（第１１発明）。
【００７０】
この第１１発明によれば、第１の撮像画像に写っている下段側の平坦面部のうち、上段側の平坦面部の段差エッジに近い部分が第２の撮像画像には写っていないというようなオクルージョンが発生していても、前記暫定的な段差エッジが、実際の段差エッジに一致もしくはほぼ一致するような状況で、上記オクルージョンに起因する誤差成分が前記第５誤差関数に極力含まれないようにすることができる。
【００７１】
ひいては、前記暫定的な段差エッジと実際の段差エッジとの一致度合いを反映するものとしての前記誤差関数の値の信頼性を高めることができる。その結果、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータの信頼性をより一層高めることができる。
【図面の簡単な説明】
【００７２】
【図１】図１（ａ），（ｂ）はそれぞれ、本発明の一実施形態におけるロボットが階段を昇ろうとしている状況、降ろうとしている状況を示す斜視図。
【図２】代表的な階段の構造を説明するための図。
【図３】図１に示すロボットに備えた演算処理装置の機能的構成を示すブロック図。
【図４】図４（ａ），（ｂ）はそれぞれ、実施形態で説明する段差部（階段）に関する平面と平面パラメータベクトルとを例示する斜視図、側面視図。
【図５】図５（ａ），（ｂ）は、それぞれ第１の撮像画像を例示する図。
【図６】図６（ａ），（ｂ）は、それぞれ第１の撮像画像を例示する図。
【図７】図３に示した段差エッジ特定手段の処理（図１（ａ）に示す状況での処理）を示すブロック図。
【図８】図７のＳ４，Ｓ５，Ｓ６の処理を説明するための図。
【図９】図７のＳ４，Ｓ５，Ｓ６の処理の他の例を説明するための図。
【図１０】図７のＳ１における暫定段差エッジパラメータの更新処理を説明するための図。
【図１１】図３に示した段差エッジ特定手段の処理（図１（ｂ）に示す状況での処理）を示すブロック図。
【図１２】図７のＳ３における領域Ａb1の設定手法の他の例を説明するための図。
【図１３】図１１のＳ２２における領域Ａa3，Ａb3の設定手法の他の例を説明するための図。
【発明を実施するための形態】
【００７３】
本発明の一実施形態を図１〜図１１を参照して以下に説明する。
【００７４】
図１（ａ），（ｂ）を参照して、本実施形態では、移動体としての脚式移動ロボット１によって階段等の段差部の段差エッジの位置及び方向を推定する場合を例にとって説明する。図示例の脚式移動ロボット１（以下、単にロボット１という）は、左右一対の２つの脚体を備える２足歩行ロボットである。
【００７５】
このロボット１には、２つの撮像画像から構成されるステレオ画像を撮像する撮像手段として、左右一対のカメラ３Ｒ，３Ｌが搭載され、これらのカメラ３Ｒ，３Ｌによって、ロボット１の周辺環境（主に、ロボット１の進行方向前方の環境）のステレオ画像を取得することが可能となっている。なお、カメラ３Ｒ，３Ｌの撮像画像は、モノトーン画像及びカラー画像のいずれの画像であってもよい。
【００７６】
そして、ロボット１は、これらのカメラ３Ｒ，３Ｌによって取得したステレオ画像等を基に、周辺環境の床形状や設置物等を認識しつつ移動する。
【００７７】
例えば、図１（ａ）に示すように、ロボット１がその進行方向前方に存在する段差部としての階段５０を昇ろうとしている状況、あるいは、図１（ｂ）に示すように、ロボット１がその進行方向前方に存在する段差部としての階段５０を降ろうとしている状況において、ロボット１は、カメラ３Ｒ，３Ｌによって取得した進行方向前方の環境のステレオ画像を基に、階段５０の各段の平坦面部５１（詳しくは、ほぼ水平な平坦面として認識し得る部分）の３次元空間中の位置及び姿勢（法線方向）や、各段の平坦面部５１の先端側エッジとしての段差エッジ５２の３次元空間中の位置及び方向を認識する。
【００７８】
ここで、ロボット１が認識する階段５０の各段の平坦面部５１の段差エッジ５２は、図１（ａ）に示すロボット１の如く、階段５０をその下段側が手前側、上段側が奥側となる視線で見た場合においては、上段側の平坦面部５１の外形の境界線のうち、手前側で横方向（階段５０の幅方向）に直線状に延在する境界線である。
【００７９】
また、図１（ｂ）に示すロボット１の如く、階段５０をその上段側が手前側、下段側が奥側となる視線で見た場合にいては、段差エッジ５２は、上段側の平坦面部５１の外形の境界線のうち、奥側で横方向（階段５０の幅方向）に直線状に延在する境界線である。
【００８０】
補足すると、図２を参照して、階段の各段の平坦面部は、一般に踏面と言われる部分である。そして、この踏面の先端部は、一般に段鼻と言われる部分である。従って、上記階段５０の各段の平坦面部５１は、換言すれば、階段５０の各段の踏面を意味し、また、上記段差エッジ５２は、換言すれば、階段５０の各段の段鼻の上面の先端側境界線を意味する。
【００８１】
なお、階段の上段側の踏面（平坦面部）の先端側と、その一段下側の下段側の踏面（平坦面部）の奥側との間での部分は、一般に蹴込みと言われる。そして、階段の各段の踏面（平坦面部）の先端部（段鼻）は、多くの場合、図２に示す如く、その先端面が、上記蹴込みの箇所に起立した姿勢で形成される面部（以降、蹴込み面部という）よりも若干突き出るように形成される。
【００８２】
但し、本実施形態で、段差エッジの推定対象として主に説明する階段では、段鼻の先端面が、蹴込み面部よりも若干突き出ている必要はない。例えば、図１（ａ），（ｂ）に簡略的に記載した階段５０のように、段鼻の先端面と蹴込み面部とが面一もしくはほぼ面一に形成されていてもよい。また、本実施形態で、段差エッジの推定対象として主に説明する階段は、蹴込みの箇所に、蹴込み面部を形成する板部材が設けられていない（蹴込み面部が存在しない）構造のものであってもよい。
【００８３】
図３を参照して、ロボット１には、ＣＰＵ、ＲＡＭ、ＲＯＭを含む電子回路ユニットにより構成された演算処理装置１０が搭載されており、カメラ３Ｒ，３Ｌによりそれぞれ撮像された撮像画像の画素値データ（撮像画像を構成する画素毎の輝度、彩度、色相等を表すデータ）が演算処理装置１０に入力されて、図示しない画像メモリに逐次更新されつつ記憶保持されるようになっている。
【００８４】
演算処理装置１０は、実装されたプログラムによって実現される機能として、ロボット１の進行方向前方に存在する平坦面部の位置及び姿勢（法線方向）を認識する平坦面部認識手段１１と、階段等の段差部の段差エッジの位置及び方向を表す段差エッジパラメータを探索的な手法によって特定する段差エッジ特定手段１２とを備えており、これらの手段１１，１２の処理によって、本発明の段差エッジ推定装置としての機能を実現する。なお、演算処理装置１０は、段差エッジ推定装置としての機能の他に、ロボット１の動作制御を行なう機能を備えていてもよい。
【００８５】
上記平坦面部認識手段１１及び段差エッジ特定手段１２のより詳細な処理を以下に説明する。
【００８６】
カメラ３Ｒ，３Ｌによって撮像されたロボット１の進行方向前方の環境のステレオ画像を構成する２つの撮像画像の画素値データが平坦面部認識手段１１に与えられる。そして、平坦面部認識手段１１は、与えられた２つの撮像画像の画素値データから、公知のステレオマッチングの手法によって、両撮像画像に捉えられている物体の各部の空間的な位置を算出する。その空間的な位置は、カメラ３Ｒ，３Ｌに対して固定されたカメラ座標系での位置である。該カメラ座標系としては、本実施形態では、図４（ａ），（ｂ）に例示する如く、カメラ３Ｒ，３Ｌのいずれか一方の光学中心、例えばカメラ３Ｒの光学中心Ｃ0に原点を設定した３軸直交座標系（ｘｙｚ座標系）Ｃcamが用いられる。
【００８７】
さらに、平坦面部認識手段１１は、算出した位置データを基に、各撮像画像（あるいはいずれか一方の撮像画像）において、空間的な高さがほぼ一定となる領域（該領域内の各部に対応する空間的な高さが互いにほぼ同一となる領域）、すなわち、ほぼ水平な面と見なせる領域が投影された画像部分を平坦面部投影領域として抽出する。
【００８８】
例えば、図１（ａ）又は（ｂ）に示した状況で、階段５０をカメラ３Ｒ，３Ｌにより撮像し、カメラ３Ｒ，３Ｌのいずれか一方、例えばロボット１の右側のカメラ３Ｒによって、図５（ａ），（ｂ）あるいは図６（ａ），（ｂ）に示す如き撮像画像が得られた場合を想定する。図５（ａ），（ｂ）は、ロボット１の進行方向前方に昇り階段が存在する状況でのカメラ３Ｒの撮像画像を例示しており、図６（ａ），（ｂ）は、ロボット１の進行方向前方に降り階段が存在する状況でのカメラ３Ｒの撮像画像を例示している。
【００８９】
この場合、カメラ３Ｒの撮像画像において、図５（ａ），（ｂ）で参照符号Ａr1，Ａr2を付した領域、あるいは、図６（ａ），（ｂ）で参照符号Ａr3，Ａr4，Ａr5，Ａr6を付した領域が平坦面部投影領域として抽出される。
【００９０】
補足すると、２つの撮像画像（ステレオ画像）から空間的な位置データが算出された各部の高さは、例えば、カメラ座標系Ｃcamでの位置データを、ロボット１の支持脚側の接地面（該脚体の足平部と床面との接触面）に設定される座標系に座標変換することで、認識することができる。この場合、座標変換は、ロボット１の各関節の変位量の検出値又は目標値に応じて行なえばよい。
【００９１】
次いで、平坦面部認識手段１１は、抽出した平坦面部投影領域に対応する平坦面部を包含する平面の空間的な位置及び姿勢（法線方向）を表す平面パラメータを算出する。この平面パラメータは、本実施形態では、平面の法線方向の単位ベクトル↑ｎを、カメラ３Ｒ，３Ｌのうちの一方、例えばカメラ３Ｒの光学中心Ｃ0と平面との間の距離ｄ（光学中心Ｃ0から平面への垂線の長さ）により除算してなるベクトル↑ｍ（＝↑ｎ／ｄ）である。以降、この↑ｍを平面パラメータベクトル↑ｍという。なお、本明細書では、“↑”をベクトルを表現する記号として使用している。
【００９２】
上記平面パラメータベクトル↑ｍは、公知の手法によって算出される。例えば、平坦面部投影領域の各部に対応する空間的な位置データを用いて、↑ｍを算出することができる。あるいは、例えば、カメラ３Ｒ，３Ｌにより撮像した２つの撮像画像（ステレオ画像）から、本願出願人が特開２００８−２５０５１６号公報にて先に提案した手法のように、射影変換（平面射影変換）を利用した探索的な演算処理によって↑ｍを算出することもできる。
【００９３】
このようにして平坦面部認識手段１１によって算出される平面パラメータベクトル↑ｍの例を図４（ａ），（ｂ）に示す。図４（ａ），（ｂ）は、例えば図１（ａ）に示した状況で、階段５０（昇り階段）をカメラ３Ｒ，３Ｌにより撮像した場合の例を示しており、図中の↑ｍａ、↑ｍｂが平坦面部認識手段１１の処理によって算出される平面パラメータベクトルである。↑ｍａは、階段５０の上段側の平坦面部５１ａを包含する上段側平面ＰＬａに対応する平面パラメータベクトル、↑ｍｂは、上段側の平坦面部５１ａの一段下側の下段側の平坦面部５１ｂを包含する下段側平面ＰＬｂに対応する平面パラメータである。
【００９４】
なお、図４（ｂ）中のｄａ，ｄｂは、それぞれカメラ３Ｒの光学中心Ｃ0と、上段側平面ＰＬａ、下段側平面ＰＬｂとの距離を示しており、上段側平面ＰＬａ、下段側平面ＰＬｂのそれぞれの法線方向の単位ベクトルをそれぞれ↑ｎａ、↑ｎｂとおくと、↑ｍａ＝↑ｎａ／ｄａ、↑ｍｂ＝↑ｎｂ／ｄｂとなる。
【００９５】
図１（ｂ）に示した状況で、階段５０（降り階段）をカメラ３Ｒ，３Ｌにより撮像した場合においても、上記と同様に、図３（ａ），（ｂ）に示した平面パラメータベクトル↑ｍａ，↑ｍｂが平坦面部認識手段１１の処理によって算出される。
【００９６】
補足すると、平面パラメータは、平面の空間的な位置および姿勢（法線方向）を表すものであればどのようなものでもよい。例えば、平面パラメータベクトル↑ｍの代わりに、平面の法線方向の単位ベクトル↑ｎと、光学中心Ｃ0から平面までの距離ｄとの組（↑ｎ，ｄ）を平面パラメータとして用いてもよい。あるいは、例えば平面上の３点の空間的な位置の組を平面パラメータとして用いることもできる。
【００９７】
以上が平坦面部認識手段１１の処理の詳細である。
【００９８】
次に、段差エッジ特定手段１２に関して説明する。段差エッジ特定手段１２は、ロボット１の進行方向前方に階段等の段差部が存在する場合に、該段差部の各段の平坦面部（ロボット１の遊脚側の脚体の足平部を着地させようとする平坦面部）の実際の段差エッジの位置及び方向を表す段差エッジパラメータを探索的に特定する演算処理を実行することで、段差部の段差エッジの空間的な（３次元空間中の）位置及び方向を推定する。この場合、段差エッジパラメータを探索的に特定する演算処理では、段差エッジパラメータを暫定的に仮決定し、その暫定的に仮決定した段差エッジパラメータを後述する評価関数Ｅ１の値に基づいて適宜修正することで、実際の段差エッジの位置及び方向の推定値を表す段差エッジパラメータとしての最適段差エッジパラメータを特定する。
【００９９】
上記段差エッジパラメータは、それによって、段差エッジの空間的な位置及び方向を規定できるものであればどのようなものでもよい。本実施形態では、例えば、段差エッジを形成する直線（段差エッジに重なる直線）の空間的な方向を示すベクトル（段差エッジと同方向のベクトル）と、該直線上の１つの点の空間的な位置との組、あるいは、段差エッジを形成する直線上の互いに異なる２つの点の空間的な位置の組により構成される。
【０１００】
なお、例えば、段差エッジを形成する空間的な直線を、カメラ３Ｒ，３Ｌのいずれか一方の撮像画像に投影することによって該撮像画像中に得られる二次元直線の、該撮像画像上での二次元的な位置及び方向を表すパラメータ（例えば、該二次元直線の二次元的な方向を示すベクトルと、該二次元直線上の１つの点の位置との組、もしくは、該二次元直線上の互いに異なる２つの点の空間的な位置の組）を、段差エッジパラメータとして採用してもよい。段差エッジは、上段側の平坦面部と同じ平面上に存在するので、該段差エッジの撮像画像中での二次元的な位置及び方向が決まれば、その位置及び方向と、上段側の平坦面部を包含する平面（上段側平面ＰＬａ）の空間的な位置及び姿勢を表す平面パラメータ（例えば前記平面パラメータベクトル）とから、段差エッジの空間的な位置及び姿勢を特定することが可能である。
【０１０１】
ここで、ロボット１の進行方向前方に階段等の段差部が存在する場合に、段差エッジ特定手段１２が、実際の段差エッジに対応する段差エッジパラメータ（最適段差エッジパラメータ）を探索的に特定するために実行する演算処理は、カメラ３Ｒ，３Ｌの視線方向（カメラ３Ｒ，３Ｌから段差部を見る向き）が、段差部の下段側の平坦面部が手前側、上段側の平坦面部が奥側となる方向である場合（例えば図１（ａ）に示す場合。以降、カメラ視線が下・上方向の場合ということがある）と、カメラ３Ｒ，３Ｌの視線方向が、段差部の上段側の平坦面部が手前側、下段側の平坦面部が奥側となる方向である場合（例えば図１（ｂ）に示す場合。以降、カメラ視線が上・下方向の場合ということがある）とで一部の処理が異なる。
【０１０２】
これは、カメラ視線が下・上方向の場合には、上段側の平坦面部とその一段下側の平坦面部との間の蹴込みの箇所の画像が各撮像画像内に含まれることとなる一方、カメラ視線が上・下方向の場合には、蹴込みの箇所の画像が各撮像画像内に含まれないこととなるからである。
【０１０３】
まず、カメラ視線が下・上方向の場合の段差エッジ特定手段１２の処理を説明する。この説明においては、段差部が階段５０であって、カメラ３Ｒの撮像画像が、図５（ａ），（ｂ）に示したような画像となる場合を主要例として段差エッジ特定手段１２の処理を説明する。この場合、図５（ａ），（ｂ）に示す平坦面部投影領域Ａr1，Ａr2にそれぞれ対応する階段５０の平坦面部は、それぞれ、図４（ａ），（ｂ）に示す上段側の平坦面部５１ａ、その一段下側の下段側の平坦面部５１ｂであるとする。なお、以降の説明では、カメラ３Ｒ，３Ｌの撮像画像のうちの一方、例えば、カメラ３Ｒの撮像画像を第１の撮像画像、カメラ３Ｌの撮像画像を第２の撮像画像と称する。また、以降の説明では、暫定的に仮決定する段差エッジパラメータを暫定段差エッジパラメータ、該暫定段差エッジパラメータにより位置及び方向が規定される直線（暫定的な段差エッジを形成する直線）を暫定段差エッジラインＬｅと称する。
【０１０４】
カメラ視線が下・上方向の場合、段差エッジ特定手段１２は、図７のブロック図に示す処理を実行するとで、実際の段差エッジに対応する段差エッジパラメータ（最適段差エッジパラメータ）を探索的に特定する。
【０１０５】
段差エッジ特定手段１２は、まず、Ｓ１の処理を実行し、暫定段差エッジパラメータの初期値を設定する。この場合、暫定段差エッジパラメータの初期値により規定される暫定段差エッジラインＬｅが、ロボット１の進行方向に対して交差する方向（進行方向を横切る方向）に延在すると共に、該暫定段差エッジラインＬｅが、カメラ３Ｒ，３Ｌによって撮像された階段５０の上段側の平坦面部５１ａ（推定しようとする段差エッジを先端に有する平坦面部５１ａ）のうちの、平坦面部認識手段１１によって抽出された平坦面部投影領域に対応する領域を通るように、暫定段差エッジパラメータの初期値が設定される。
【０１０６】
例えば、図４（ａ）に二点鎖線で示す直線Ｌe(0)が、暫定段差エッジパラメータの初期値により規定される暫定段差エッジラインＬｅとされる。この場合、第１の撮像画像上においては、上記暫定段差エッジラインＬｅ（＝Ｌe(0)）を第１の撮像画像に投影してなる線（二次元直線）は、図５（ａ）に示すラインＬep(0)となる。なお、以降、暫定段差エッジラインＬｅを第１の撮像画像に投影してなる二次元直線を、画像上暫定段差エッジラインＬepという。
【０１０７】
上記暫定段差エッジパラメータの初期値のより具体的な設定手法としては、例えば次のような手法が挙げられる。その１つの手法では、例えば、階段５０の上段側の平坦面部５１ａに対応して平坦面部認識手段１１によって抽出された平坦面部投影領域Ａr1の内部に１つの点Ｆ１を設定し、その点Ｆ１に対応する空間上の位置の点（上段側の平坦面部５１ａの点）から下段側の平坦面部５１ｂを包含する平面ＰＬｂに降ろした垂線を第１の撮像画像に投影してなる線Ｆ２を求める。さらに、第１の撮像画像中で、当該投影線Ｆ２に直交し、且つ、上記点Ｆ１を通る直線Ｆ３を求める。そして、暫定段差エッジパラメータの初期値により規定される画像上暫定エッジラインＬep(0)が、上記直線Ｆ３に合致するように、暫定段差エッジパラメータの初期値を設定する。
【０１０８】
あるいは、他の１つの手法では、第１の撮像画像中で、階段５０の上段側の平坦面部５１ａに対応する平坦面部投影領域Ａr1内で、ロボット１の進行方向に延在し、横方向に間隔を有する２つの所定長の線分Ｇ１，Ｇ２を設定し、それらの線分Ｇ１，Ｇ２の中点を結ぶ直線Ｇ３を求める。そして、暫定段差エッジパラメータの初期値により規定される画像上暫定エッジラインＬep(0)が、上記直線Ｇ３に合致するように、暫定段差エッジパラメータの初期値を設定する。
【０１０９】
なお、暫定段差エッジパラメータの初期値の設定の仕方は、上記の例に限らず、種々様々な手法を採用できる。
【０１１０】
補足すると、本実施形態では、段差エッジ特定手段１２は、第１の撮像画像に、図５（ａ），（ｂ）に破線で示す如き方形状の枠内領域ＡＲ１を画像処理領域として設定し、この画像処理領域ＡＲ１での撮像画像を、実際の段差エッジに対応する段差エッジパラメータを探索的に特定するための演算処理に利用する。この場合、第１の撮像画像において、画像処理領域ＡＲ１の上部が、階段５０の上段側の平坦面部５１ａに対応する平坦面部投影領域Ａr1に存在し、且つ、画像処理領域ＡＲ１の下部が、階段５０の下段側の平坦面部５１ｂの平坦面部投影領域Ａr2に存在するように、該画像処理領域ＡＲ１が第１の撮像画像中に設定される。なお、画像処理領域ＡＲ１の横方向の幅は、例えば、概ねロボット１の通過領域と同程度の幅となるように設定される。
【０１１１】
そして、暫定段差エッジパラメータの初期値は、それにより規定される画像上暫定段差エッジラインＬep(0)の画像処理領域ＡＲ１内の部分が、図５（ａ）に示す如く、階段５０の上段側の平坦面部５１ａに対応する平坦面部投影領域Ａr1内に含まれるように設定することが望ましい。
【０１１２】
なお、Ｓ１の処理では、後に、暫定段差エッジパラメータの更新処理も行なうが、これについては後述する。
【０１１３】
段差エッジ特定手段１２は、次に、図７のＳ２の処理と、Ｓ３の処理とを実行する。このうち、Ｓ２の処理では、段差エッジ特定手段１２は、Ｓ１で設定した暫定段差エッジパラメータにより規定される暫定段差エッジラインＬｅを含む（該暫定段差エッジラインＬｅで上段側の平坦面部５１ａを包含する上段側平面ＰＬａと交差する）と共に、上段側の平坦面部５１ａを包含する上段側平面ＰＬａに対して起立した姿勢となる起立平面ＰＬｃ（図４（ａ），（ｂ）を参照）の位置及び姿勢を表す平面パラメータを設定する。
【０１１４】
この場合、起立平面ＰＬｃの平面パラメータは、上段側平面ＰＬａ及び下段側平面ＰＬｂの平面パラメータとしての平面パラメータベクトル↑ｍａ，↑ｍｂと同様に、起立平面ＰＬｃの法線方向の単位ベクトル↑ｎｃを、カメラ３Ｒの光学中心Ｃ0と該起立平面ＰＬｃとの距離ｄｃ（図４（ｂ）を参照）により除算してなる平面パラメータベクトル↑ｍｃ（＝↑ｎｃ／ｄｃ）である。
【０１１５】
ここで、起立平面ＰＬｃは、階段の段鼻の先端面を包含する平面に相当するものである。そして、階段の各段の段鼻の先端面は、一般に、その段の平坦面部（踏面）に対してほぼ垂直に起立した面となっている。
【０１１６】
そこで、本実施形態では、段差エッジ特定手段１２は、Ｓ２において、起立平面ＰＬｃが上段側平面ＰＬａに対して垂直に起立し、且つ、Ｓ１で設定した暫定段差エッジパラメータにより規定される暫定段差エッジラインＬｅを含むように、起立平面パラメータベクトル↑ｍｃを設定する。
【０１１７】
例えば、暫定段差エッジラインＬｅが図４（ａ）に示したＬe(0)である場合、図４（ａ），（ｂ）に示すように、起立平面パラメータベクトル↑ｍｃが設定される。
【０１１８】
この場合、上記起立平面ＰＬｃの空間的な位置及び姿勢（カメラ座標系Ｃcamでの位置及び姿勢）は、カメラ座標系Ｃcamでの上段側平面ＰＬａの姿勢と、暫定段差エッジラインＬｅの位置及び姿勢により一義的に定まることとなる。そこで、Ｓ２の処理では、段差エッジ特定手段１２は、Ｓ１で設定した暫定段差エッジパラメータと、平坦面部認識手段１１で先に算出した上段側平面パラメータベクトル↑ｍａとに応じて、起立平面パラメータベクトル↑ｍｃを設定する。
【０１１９】
なお、上段側平面ＰＬａに対する起立平面ＰＬｃの傾斜角度は、必要に応じて適宜、９０°以外の角度に設定してもよい。例えば、段差エッジを推定しようとする階段の各段鼻の先端面が、踏面に対してほぼ同じ傾斜角度で傾斜した姿勢となっており、その傾斜角度（平均的な傾斜角度）が事前の計測等により判明しているような場合には、その傾斜角度だけ、起立平面ＰＬｃが上段側平坦面ＰＬａに対して傾斜させた姿勢となるように、起立平面パラメータを設定するようにしてもよい。
【０１２０】
前記Ｓ３の処理では、段差エッジ特定手段１２は、第１の撮像画像中に、後述の射影変換（平面射影変換）を行なうために用いる３つの領域Ａa1，Ａb1，Ａc1を、Ｓ１で設定した暫定段差エッジパラメータに応じて設定する。
【０１２１】
第１の撮像画像中の３つの領域Ａa1，Ａb1，Ａc1は、第１の撮像画像中に前記した如く設定した画像処理領域ＡＲ１内で図５（ａ），（ｂ）に例示する如く設定される。これらの３つの領域Ａa1，Ａb1，Ａc1は、それぞれ、本発明における第２の領域、第３の領域、第１の領域に相当するものであり、これらの設定の仕方の基本的な指針は次の通りである。
【０１２２】
すなわち、領域Ａa1，Ａc1については、基本的には、第１の撮像画像において、画像上暫定段差エッジラインＬepが領域Ａa1，Ａc1の境界線となると共に、領域Ａc1が、上段側の平坦面部５１ａと下段側の平坦面部５１ｂとの間の蹴込みの箇所（これは、本発明における上下段間部分に相当する）と、上段側の平坦面部５１ａとのうちの蹴込みの箇所寄りの領域、領域Ａa1が、上段側の平坦面部５１ａ寄りの領域となるように、これらの領域Ａa1，Ａc1の画像処理領域ＡＲ１内で設定される。
【０１２３】
従って、図５（ａ），（ｂ）に示す第１の撮像画像においては、領域Ａa1，Ａc1は、画像処理領域ＡＲ１内で、画像上暫定段差エッジラインＬep（図５（ａ）ではＬep(0)、図５（ｂ）ではＬep(n）)を間に挟んで上下に隣接するように設定される。
【０１２４】
この場合、領域Ａa1の上側の境界線は、画像処理領域ＡＲ１の上端の境界線に一致するものとされる。すなわち、画像処理領域ＡＲ１の画像上暫定段差エッジラインＬepの上側の全体の領域が、領域Ａa1として設定される。
【０１２５】
一方、領域Ａc1の下側の境界線（画像上暫定段差エッジラインＬep（Ｌep(0)，Ｌep(n)）と反対側の境界線）については、図５（ａ），（ｂ）に例示する如く、画像上暫定段差エッジラインＬepと所定の間隔Δｃを存して平行に延在するように設定される。その間隔Δｃは、本実施形態では、段鼻が踏面の先端部に形成された一般的な階段をカメラ３Ｒ，３Ｌにより撮像した場合に、それぞれの撮像画像における段鼻の厚みとほぼ同程度になるようにあらかじめ設定されている。
【０１２６】
なお、撮像画像における段鼻の厚みは、カメラ３Ｒ，３Ｌから段鼻への距離によって変化するので、例えば、暫定段差エッジラインの近辺での上段側の平坦面部５１ａの距離（カメラ３Ｒ又は３Ｌからの距離）に応じて、領域Ａc1の上記間隔Δｃを可変的に設定するようにしてもよい。
【０１２７】
また、領域Ａb1については、基本的には、第１の撮像画像において、領域Ａc1よりも下段側の平坦面部５１ｂ寄りの位置で該下段側の平坦面部５１ｂの少なくとも一部分を含むと共に、領域Ａb1の領域Ａc1側の境界線の第１の撮像画像中での位置及び方向が、画像上暫定段差エッジラインＬepの位置及び方向に応じて規定されるように、該領域Ａb1が画像処理領域ＡＲ１内で設定される。
【０１２８】
この場合、領域Ａb1の上側の境界線は、領域Ａc1の下側の境界線と所定の間隔Δｂを存して平行に延在するように設定される。その間隔Δｂは、本実施形態では、画像上暫定段差エッジラインＬepが、第１の撮像画像における実際の段差エッジ５２の線に一致もしくはほぼ一致する場合（図５（ｂ）に示す場合）に、階段５０の上段側の平坦面部５１ａと下段側の平坦面部５１ｂとの間の蹴込みの箇所が、領域Ａb1から除外されると共に、領域Ａb1の領域Ａc1側の境界線が下段側の平坦面部５１ｂの画像内に存在するようにあらかじめ設定されている。
【０１２９】
なお、領域Ａc1に係わる上記間隔Δｃと同様に、領域Ａb1に係わる上記間隔Δｂを、例えば、下段側の平坦面部５１ｂの奥側部分の距離（カメラ３Ｒ又は３Ｌからの距離）に応じて可変的に設定するようにしてもよい。
【０１３０】
そして、領域Ａb1の下側の境界線は、画像処理領域ＡＲ１の下端の境界線に一致するものとされる。すなわち、画像処理領域ＡＲ１のうち、上記の如く設定される領域Ａb1の上側の境界線の下側の全体の領域が、領域Ａb1として設定される。
【０１３１】
なお、本実施形態では、上記Ｓ３の処理によって、本発明における評価関数算出用領域設定手段を実現される。
【０１３２】
以上の如く、Ｓ２、Ｓ３の処理を実行した後、段差エッジ特定手段１２は、次に、Ｓ４、Ｓ５、Ｓ６の処理を実行する。このＳ４、Ｓ５、Ｓ６では、段差エッジ特定手段１２は、それぞれ、Ｓ３で設定した第１の撮像画像の領域Ａa1、領域Ａb1、領域Ａc1にそれぞれ対応する第２の撮像画像中の領域の画像に射影変換（平面射影変換）を施した画像を算出する。以降の説明では第１の撮像画像の領域Ａa1、領域Ａb1、領域Ａc1にそれぞれ対応する第２の撮像画像中の領域を、それぞれ、領域Ａa2、領域Ａb2、領域Ａc2と表記する。
【０１３３】
ここで、上記射影変換は、一般的に言えば、ステレオ画像を構成する２つの撮像画像に、階段の踏面の如き平坦面部（平面部分）を撮像した場合に、一方の撮像画像中における当該平坦面部の画像を、他方の撮像画像における当該平坦面部の画像に変換するものである。その変換を行なう射影変換行列Ｐは、カメラ３Ｒ，３Ｌの２つの撮像画像のそれぞれの画像座標（各撮像画像における各画素の位置を表す二次元座標）を同次座標で表現した場合に、公知のように次式（１−１）により表される行列である。
【０１３４】
Ｐ＝Ｒ＋↑ｔ・↑ｍT ……（１−１）
式（１−１）におけるＲは、カメラ３Ｒ，３Ｌのそれぞれのカメラ座標系の間の回転行列、↑ｔはそれらのカメラ座標系の間の並進移動ベクトル（ここでは縦ベクトル）、↑ｍは前記した如く定義される平面パラメータベクトル（ここでは縦ベクトル）であり、↑ｍTは、↑ｍの転置を意味する。
【０１３５】
この場合、回転行列Ｒ及び並進移動ベクトル↑ｔは、カメラ３Ｒ，３Ｌの相互の相対的な位置関係及び姿勢関係に依存して定まるものであり、本実施形態ではあらかじめ特定されている。
【０１３６】
補足すると、第１の撮像画像から第２の撮像画像への射影変換行列と、これの逆変換の射影変換行列（第２の撮像画像から第１の撮像画像への射影変換行列）とは、いずれも、式（１−１）の右辺の形で表現されるものの、上記Ｒ，↑ｔ，↑ｍの値は、それぞれの射影変換行列で異なる。
【０１３７】
具体的には、式（１−１）により表現される射影変換行列Ｐを第１の撮像画像及び第２の撮像画像のうちの一方から他方へのへの射影変換行列とした場合、その逆変換の射影変換行列（以降、これをＰ-1と表記する）は、次式（１−２）の但し書きの式（１−２ａ）〜（１−２ｄ）により定義されるＲ’、↑ｔ’、↑ｍ’、↑ｄ’を用いて、上記式（１−１）と同様の形の式（１−２）により表される。
【０１３８】
Ｐ-1＝Ｒ'＋↑ｔ’・(↑ｍ’)T ……（１−２）
但し、
Ｒ’≡ＲT ……（１−２ａ）
↑ｔ’≡−ＲT・↑ｔ ……（１−２ｂ）
↑ｍ’≡Ｒ・↑ｍ／ｄ’ ……（１−２ｃ）
↑ｄ’≡ｄ・（１＋↑ｍT・ＲT・↑ｔ） ……（１−２ｄ）
以降の本実施形態の説明では、便宜上、第１の撮像画像から第２の撮像画像への射影変換行列を式（１−１）により表される射影変換行列Ｐとし、第２の撮像画像から第１の撮像画像への射影変換行列を式（１−２）により表される射影変換行列Ｐ-1とする。
【０１３９】
以上を前提としてＳ４，Ｓ５，Ｓ６の処理を、図８を参照しつつ以下に具体的に説明する。なお、図８は、Ｓ４、Ｓ５，Ｓ６の処理を包括的に説明するための図であり、図中の符号中の「ｘ」は、Ｓ４の処理では「ａ」、ｓ５の処理では「ｂ」、Ｓ６の処理では「ｃ」を意味するものであるとする。
【０１４０】
Ｓ４では、段差エッジ特定手段１２は、平坦面部認識手段１１により先に算出された上段側平面ＰＬａの平面パラメータベクトル↑ｍａを式（１−１）の↑ｍに代入することで射影変換行列Ｐａを決定し、このＰａを用いて、第１の撮像画像の領域Ａa1に対応する第２の撮像画像中の領域Ａa2を求める。具体的には、図８に示す如く、第１の撮像画像の領域Ａa1の外形枠にＰａによる射影変換を施すことによって、第２の撮像画像中の領域Ａa2を求める。
【０１４１】
さらに、段差エッジ特定手段１２は、前記式（１−２）及びそのただし書きの式に基づいて、上記Ｐａの逆変換の射影変換行列Ｐａ-1を決定し、このＰａ-1による射影変換を第２の撮像画像中の領域Ａa2内の画像に対して施す。これにより、第１の撮像画像に設定した領域Ａa1に対応する第２の撮像画像の領域Ａa2に、上段側平面ＰＬａの平面パラメータベクトル↑ｍａに応じた射影変換を施してなる画像が算出される。
【０１４２】
上記と同様に、Ｓ５では、段差エッジ特定手段１２は、平坦面部認識手段１１により先に算出された下段側平面ＰＬｂの平面パラメータベクトル↑ｍｂを式（１−１）の↑ｍに代入することで射影変換行列Ｐｂを決定し、このＰｂによる射影変換を第１の撮像画像の領域Ａb1の外形枠に施すことによって、Ａb1に対応する第２の撮像画像中の領域Ａb2を求める。そして、段差エッジ特定手段１２は、上記Ｐｂの逆変換の射影変換行列Ｐｂ-1を決定し、このＰｂ-1による射影変換を第２の撮像画像中の領域Ａb2内の画像に対して施す。これにより、第１の撮像画像に設定した領域Ａb1に対応する第２の撮像画像の領域Ａb2に、下段側平面ＰＬｂの平面パラメータベクトル↑ｍｂに応じた射影変換を施してなる画像が算出される。
【０１４３】
また、Ｓ６では、Ｓ２の処理により算出した起立平面ＰＬｃの平面パラメータベクトル↑ｍｃを式（１−１）の↑ｍに代入することで射影変換行列Ｐｃを決定し、このＰｃによる射影変換を第１の撮像画像の領域Ａc1の外形枠に施すことによって、Ａc1に対応する第２の撮像画像中の領域Ａc2を求める。そして、段差エッジ特定手段１２は、上記Ｐｃの逆変換の射影変換行列Ｐｃ-1を決定し、このＰｂ-1による射影変換を第２の撮像画像中の領域Ａc2内の画像に対して施す。これにより、第１の撮像画像に設定した領域Ａc1に対応する第２の撮像画像の領域Ａc2に、起立平面ＰＬｃの平面パラメータベクトル↑ｍｃに応じた射影変換を施してなる画像が算出される。
【０１４４】
補足すると、本実施形態では、Ｓ４、Ｓ５、Ｓ６のそれぞれの処理において、第１の撮像画像に設定した各領域Ａx1（ｘ＝ａ又はｂ又はｃ。以降、同様）に対応する第２の撮像画像中の領域Ａx2を、平面パラメータベクトル↑ｍｘに応じて決定した射影変換行例Ｐｘによる射影変換によって算出した上で、第２の撮像画像中の領域Ａx2内の画像に、Ｐｘの逆変換の射影変換行列Ｐｘ-1による射影変換を施すことによって、第１の撮像画像の領域Ａx1に対応する第２の撮像画像中の領域Ａx2の画像の射影変換画像を求めるようにした。
【０１４５】
但し、第１の撮像画像の各領域Ａx1に対応する第２の撮像画像中の領域Ａx2そのものを求めたり、射影変換行列Ｐｘ-1による射影変換を第２の撮像画像に対して実際に行なうことを必須とするものではなく、結果的に、第１の撮像画像の領域Ａx1に対応する第２の撮像画像中の領域Ａx2の画像の射影変換画像を求めることができる処理であれば、上記の手法と異なる手法を採用してもよい。
【０１４６】
例えば、図９を参照して、第１の撮像画像中の各領域Ａx1内の各画素位置Ｑ１に対応する第２の撮像画像上の画素位置Ｑ２を、平面パラメータベクトル↑ｍｘに応じて決定した射影変換行例Ｐｘによる射影変換によって算出し、第１の撮像画像中の領域Ａx1の各画素位置Ｑ１の画素値を、その画素位置に対応して算出した第２の撮像画像上の画素位置Ｑ２における該第２の撮像画像の画素値で置き換える。このようにしても、結果的に、第１の撮像画像の領域Ａx1に対応する第２の撮像画像中の領域Ａx2の画像の射影変換画像を得ることができる。従って、第１の撮像画像の各領域Ａx1に対応する第２の撮像画像中の領域Ａx2そのものを求めたり、射影変換行列Ｐｘ-1による射影変換を第２の撮像画像に対して実際に行なうことを実行せずとも、第１の撮像画像の領域Ａx1に対応する第２の撮像画像中の領域Ａx2の画像の射影変換画像を得ることができる。
【０１４７】
以上が、本実施形態におけるＳ４、Ｓ５、Ｓ６の処理である。
【０１４８】
ここで、暫定段差エッジパラメータにより規定される暫定段差エッジラインＬｅが、実際の段差エッジに合致する場合には、第１の撮像画像の領域Ａa1，Ａb1，Ａc1は、それぞれ、その全体もしくは大部分が、上段側平面ＰＬａ上の部分の画像、下段側平面ＰＬｂ上の部分の画像、起立平面ＰＬｃ上の部分の画像に相当するものとなる。これは、領域Ａa1，Ａb1，Ａc1を前記した如く設定することによって、領域Ａa1，Ａb1，Ａc1は、暫定段差エッジパラメータにより規定される暫定段差エッジラインＬｅが、実際の段差エッジに合致している場合に、それぞれ、上段側の平坦面部５１ａの部分の画像、下段側の平坦面部５１ｂの部分の画像、段鼻の先端面の部分の画像となるように設定されるからである（図５（ｂ）を参照）。
【０１４９】
このため、暫定段差エッジラインＬｅが、実際の段差エッジに合致する場合には、第１の撮像画像の領域Ａa1，Ａb1，Ａc1にそれぞれ対応する第２の撮像画像中の領域Ａa2，Ａb2，Ａc2の各画像を射影変換した場合に得られる画像（以降、射影変換画像という）は、それぞれ、第１の撮像画像の領域Ａa1，Ａb1，Ａc1のそれぞれの実際の画像に一致もしくはほぼ一致することとなる。
【０１５０】
一方、暫定段差エッジラインＬｅが、実際の段差エッジと合致していない場合には、第２の撮像画像の領域Ａa2，Ａb2，Ａc2の各画像の射影変換画像は、それぞれ、第１の撮像画像の領域Ａa1，Ａb1，Ａc1の実際の画像に対して差異を生じることとなる。そして、その差異の度合いは、暫定段差エッジラインＬｅと実際の段差エッジとの一致度合いに応じて変化する。
【０１５１】
そこで、本実施形態では、段差エッジ特定手段１２は、以下に説明するＳ７〜Ｓ１０の処理によって、暫定段差エッジラインＬｅと実際の段差エッジとの一致度合いに応じて値が変化する所定の評価関数を、第１の撮像画像の領域Ａa1，Ａb1，Ａc1の各画像の画素値データと、第２の撮像画像の領域Ａa2，Ａb2，Ａc2の各画像の射影変換画像の画像値データとから算出する。そして、段差エッジ特定手段１２は、この評価関数によって暫定段差エッジラインＬｅと実際の段差エッジとの一致度合いを評価し、その評価結果に応じて暫定段差エッジパラメータを適宜修正することによって、該評価関数の値を最小化する段差エッジパラメータ（評価関数最小化段差エッジパラメータ）を探索的に特定する。このようにして特定される評価関数最小化段差エッジパラメータが、実際の段差エッジの位置及び方向を表す段差エッジパラメータとして得られる。
【０１５２】
上記Ｓ７〜Ｓ１０の処理を具体的に説明すると、段差エッジ特定手段１２は、Ｓ４〜Ｓ６の処理に続いて、Ｓ７〜Ｓ９の処理を実行する。Ｓ７では、段差エッジ特定手段１２は、第１の撮像画像の領域Ａa1の画像の画素値データと、第２の撮像画像の領域Ａa2の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Ｄａ１を次式（２）により算出する。
【０１５３】
また、Ｓ８では、段差エッジ特定手段１２は、第１の撮像画像の領域Ａb1の画像の画素値データと、第２の撮像画像の領域Ａb2の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Ｄｂ１を次式（３）により算出する。
【０１５４】
また、Ｓ９では、段差エッジ特定手段１２は、第１の撮像画像の領域Ａc1の画像の画像値データと、第２の撮像画像の領域Ａc2の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Ｄｃ１を次式（４）により算出する。
【０１５５】
これらの誤差関数Ｄａ１，Ｄｂ１，Ｄｃ１はそれぞれ、本発明における第２誤差関数、第３誤差関数、第１誤差関数に相当するものである。
【０１５６】
【数１】
式（２）、（３）、（４）において、Ｉ1[u]は、第１の撮像画像の座標位置ｕでの画素値、Ｉ2[w(u ;↑mx)]（但し、↑ｍｘ＝↑ｍａ又は↑ｍｂ又は↑ｍｃ）は、第２の撮像画像の↑ｍｘに応じた射影変換によってから得られる射影変換画像において、第１の撮像画像の座標位置ｕに対応する座標位置w(u ;↑mx)での画素値である。また、式（２）におけるΣ、式（３）におけるΣ、式（４）におけるΣは、それぞれ、第１の撮像画像の領域Ａa1，Ａb1，Ａc1における積算演算を意味する。
【０１５７】
なお、画素値Ｉ1[u]，Ｉ2[w(u ;↑mx)]は、一般的には、各画素の輝度値でよいが、第１の撮像画像及び第２の撮像画像が、カラー画像である場合には、彩度や色相等を使用することもできる。
【０１５８】
また、式（２）のＮa1、式（３）のＮb1、式（４）のＮc1はそれぞれ、領域Ａa1，Ａb1，Ａc1の画素数である。
【０１５９】
上記式（２）により算出される誤差関数Ｄａ１は、第１の撮像画像の領域Ａc1の画像の画素値分布と、第２の撮像画像の領域Ａc2の画像の射影変換画像の画素値分布とが一致していれば最小となり、両画素値分布の誤差が大きいほど、誤差関数Ｄａ１の値が大きくなる。このことは、誤差関数Ｄｂ１、Ｄｃ１についても同様である。
【０１６０】
なお、本実施形態では、誤差関数Ｄａ１を、Ｉ1[u]とＩ2[w(u ;↑ma)]との差の２乗値を領域Ａa1で積算したものとしたが、誤差関数Ｄａ１は、第１の撮像画像の領域Ａa1の画像の画素値分布と、第２の撮像画像の領域Ａa2の画像の射影変換画像の画素値分布との誤差の程度を表現できるものであれば、その他の形態のものであってもよい。例えば、Ｉ1[u]とＩ2[w(u ;↑ma)]との差の絶対値を領域Ａa1で積算したものを誤差関数Ｄａ１として使用してもよい。このことは、誤差関数Ｄｂ１，Ｄｃ１についても同様である。
【０１６１】
次いで、段差エッジ特定手段１２は、Ｓ１０の処理を実行する。このＳ１０では、段差エッジ特定手段１２は、Ｓ７〜Ｓ９でそれぞれ算出した誤差関数Ｄａ１，Ｄｂ１，Ｄｃ１の値を、次式（５）により線形結合することによって、評価関数Ｅ１の値を算出する。
【０１６２】
Ｅ１＝Ｄａ１＋Ｄｂ１＋Ｄｃ１ ……式（５）
この場合、第１の撮像画像の領域Ａa1，Ａb1，Ａc1は、暫定段差エッジパラメータに依存して設定される領域であるので、上記評価関数Ｅ１も、暫定段差エッジパラメータに依存する（暫定エッジパラメータの関数となる）。そして、前記した如く、暫定段差エッジラインＬｅが、実際の段差エッジに合致する場合には、第１の撮像画像の領域Ａa1，Ａb1，Ａc1の各画像が、それぞれ、第２の撮像画像の領域Ａa2，Ａb2，Ａc2のそれぞれの画像の射影変換画像に一致もしくはほぼ一致するので、誤差関数Ｄａ１，Ｄｂ１，Ｄｃ１の値がそれぞれ最小となる。ひいては、評価関数Ｅ１の値が最小となる。
【０１６３】
なお、式（５）の線形結合は、単純な加算演算であるが、Ｄａ１，Ｄｂ１，Ｄｃ１にそれぞれ重み係数を付して、Ｄａ１，Ｄｂ１，Ｄｃ１を線形結合することによって、評価関数Ｅ１を算出するようにしてもよい。
【０１６４】
上記の如く評価関数Ｅ１を算出した後、段差エッジ特定手段１２は、次に、Ｓ１において、上記評価関数Ｅ１の値を最小化する（“０”に近づける）ように、暫定段差エッジパラメータを更新する。
【０１６５】
この更新処理は、例えば次のように行われる。なお、ここでの説明では、更新前の最新の暫定段差エッジパラメータを更新前暫定段差エッジパラメータ、更新後の暫定段差エッジパラメータを更新後暫定段差エッジパラメータという。
【０１６６】
以下説明すると、段差エッジ特定手段１２は、暫定段差エッジラインＬｅの空間的な位置及び方向の一方又は両方、あるいは、第１の撮像画像における画像上暫定段差エッジラインＬepの二次元的な位置及び方向の一方又は両方を所定の微小量だけ、更新前暫定段差エッジパラメータに対応する値から変更し、その変更後のＬｅ又はＬepに対応する段差エッジパラメータを、観測用段差エッジパラメータとする。
【０１６７】
そして、段差エッジ特定手段１２は、前記画像処理領域ＡＲ１を、図１０に示すように第１の撮像画像の横方向に分割してなる複数の微小領域δＡＲのそれぞれにおいて、上記観測用段差エッジパラメータを更新前暫定段差エッジパラメータの代わりに用いて前記Ｓ３〜Ｓ１０と同様の処理を実行することによって、各微小領域δＡＲ毎の評価関数Ｅ１の値Ｅ１δを算出する。
【０１６８】
そして、段差エッジ特定手段１２は、各微小領域δＡＲ毎の評価関数Ｅ１の値Ｅ１δと、更新前暫定段差エッジパラメータに対応するＳ１０の処理で先に算出した評価関数Ｅ１の値との偏差に応じて、各微小領域δＡＲ毎に、画像上暫定段差エッジラインＬepの二次元的な位置（第１の撮像画像の縦方向での位置）のずらし量（更新前暫定段差エッジパラメータにより規定される画像上段差エッジパラメータＬepからのずらし量）を決定する。そのずらし量は、例えば、上記偏差の符号（極性）に応じて、あるいは、該偏差に所定値のゲインを乗じることで決定される。
【０１６９】
次いで、段差エッジ特定手段１２は、各微小領域δＡＲ毎に、図１０に示す如く、更新前暫定段差エッジパラメータにより規定される画像上暫定段差エッジラインＬepとしての更新前Ｌep上の点（例えば、各微小領域δＡＲでの更新前Ｌepの中央点）から上記ずらし量だけ第１の撮像画像の縦方向にずらした点Ｐδを算出する。
【０１７０】
次いで、段差エッジ特定手段１２は、各微小領域δＡＲ毎の点Ｐδを近似的に結ぶ直線を、更新後暫定段差エッジパラメータにより規定される暫定段差エッジラインＬep（更新後Ｌep）とし、該更新後Ｌepを最小二乗法により算出する。そして、段差エッジ特定手段１２は、この更新後Ｌepに対応する暫定段差エッジパラメータを更新後暫定段差エッジパラメータとして決定する。
【０１７１】
以上が、Ｓ１における暫定段差エッジパラメータの更新処理である。
【０１７２】
なお、暫定段差エッジパラメータの更新の仕方は、上記した手法に限られるものでなく、種々様々な公知の探索手法を利用できる。例えば、観測用段差エッジパラメータを複数種類、設定し、それぞれの観測用段差エッジパラメータを暫定段差エッジパラメータの代わりに用いて、Ｓ２〜Ｓ１０と同じ処理を実行することで、各観測用段差エッジパラメータ毎の評価関数Ｅ１の値を算出する。そして、それらの評価関数Ｅ１の値と、更新前暫定段差エッジパラメータに応じて算出した評価関数Ｅ１の値との偏差を基に、最急降下法等の勾配法の手法によって、評価関数Ｅ１の値を“０”に近づける更新後暫定段差エッジパラメータを決定するようにしてもよい。
【０１７３】
段差エッジ特定手段１２は、以上の如く暫定段差エッジパラメータを更新するＳ１の処理と、それに続くＳ２〜Ｓ１０の処理とを繰り返す。そして、段差エッジ特定手段１２は、ｎ回目のＳ１０の処理で算出した評価関数Ｅ１の値と、ｎ−１回目のＳ１０の処理で算出した評価関数Ｅ１の値とを比較し、それらの値の差の絶対値が、“０”近傍の所定の設定値よりも小さくなった場合にｎ回目のＳ２〜Ｓ１０の処理で使用した暫定段差エッジパラメータを、実際の段差エッジ５２の位置及び方向を表す最適段差エッジパラメータとして決定する。
【０１７４】
これにより、評価関数Ｅ１の値を最小化するように最適段差エッジパラメータが探索的に特定されることとなる。例えば、ｎ回目の暫定段差エッジパラメータにより規定される画像上暫定段差エッジラインＬepが図５（ｂ）に示すＬep(n)である場合、このＬep(n)に対応する暫定段差エッジパラメータが最適段差エッジパラメータとして決定される。
【０１７５】
なお、ｎ回目のＳ１０の処理で算出した評価関数Ｅ１の値が、“０”近傍の所定の設定値よりも小さい場合、あるいは、暫定段差エッジパラメータを更新するＳ１の処理と、それに続くＳ２〜Ｓ１０の処理との繰り返し回数ｎが、所定回数に達した場合に、ｎ回目の処理で使用した暫定段差エッジパラメータを、実際の段差エッジ５２の位置及び方向を表す最適段差エッジパラメータとして決定するようにしてもよい。
【０１７６】
以上が、カメラ視線が下・上方向の場合における段差エッジ特定手段１２の処理である。
【０１７７】
次に、カメラ視線が上・下方向の場合における段差エッジ特定手段１２の処理を説明する。この説明においては、段差部が階段５０であって、カメラ３Ｒの撮像画像（第１の撮像画像）が、図６（ａ），（ｂ）に示したような画像となる場合を主要例として段差エッジ特定手段１２の処理を説明する。この場合、図６（ａ），（ｂ）に示す平坦面部投影領域Ａr3，Ａr4にそれぞれ対応する階段５０の平坦面部は、それぞれ、図４（ａ），（ｂ）に示す上段側の平坦面部５１ａ、その一段下側の下段側の平坦面部５１ｂであるとする。
【０１７８】
カメラ視線が上・下方向の場合には、図６（ａ），（ｂ）に例示した第１の撮像画像のように、カメラ３Ｒ，３Ｌの各撮像画像には、階段５０の段鼻の先端面に相当する面（段差エッジから垂下する面）が写らない。
【０１７９】
このため、この場合には、段差エッジ特定手段１２は、図７のブロック図に示した処理から、起立平面ＰＬｃに関連する処理を省略してなる図１１のブロック図に示す処理によって、実際の段差エッジの位置及び方向の推定値を表す最適段差エッジパラメータを探索的に特定する。
【０１８０】
以下説明すると、段差エッジ特定手段１２は、まず、Ｓ２１の処理を実行し、暫定段差エッジパラメータの初期値を設定する。この暫定段差エッジパラメータの初期値の設定の仕方は、前記Ｓ１の処理と同じでよい。この場合、画像上暫定段差エッジラインＬepが、例えば、図６（ａ）に示したＬep(0)となるような暫定段差エッジパラメータの初期値が設定される。但し、カメラ視線が上・下方向の場合では、暫定段差エッジパラメータの初期値により規定される画像上暫定段差エッジラインＬep(0)が、上段側の平坦面部５１ａに対応する平坦面部投影領域Ａr3から逸脱するように設定されていてもよい。
【０１８１】
次いで、段差エッジ特定手段１２は、Ｓ２２の処理を実行し、第１の撮像画像中の画像処理領域ＡＲ２内に、射影変換（平面射影変換）に用いる２つの領域Ａa3，Ａb3を、Ｓ２１で設定した暫定段差エッジパラメータに応じて設定する。
【０１８２】
ここで、Ｓ２２の処理で使用する画像処理領域ＡＲ２は、図６（ａ），（ｂ）に破線で示すように設定される方形状の枠内領域である。この画像処理領域ＡＲ２は、その下部が、階段５０の上段側の平坦面部５１ａに対応する平坦面部投影領域Ａr1に存在し、且つ、画像処理領域ＡＲの上部が、階段５０の下段側の平坦面部５１ｂの平坦面部投影領域Ａr2に存在するように第１の撮像画像中に設定される。また、この画像処理領域ＡＲ２の横方向の幅は、例えば、概ねロボット１の通過領域と同程度の幅となるように設定される。
【０１８３】
そして、この画像処理領域ＡＲ２内に設定する２つの領域Ａa3，Ａb3はそれぞれ、本発明における第４領域、第５の領域に相当するものであり、これらは、次のように設定される。
【０１８４】
すなわち、第１の撮像画像において、画像上暫定段差エッジラインＬepが領域Ａa3，Ａb3の境界線となると共に、領域Ａa3が、上段側の平坦面部５１ａと下段側の平坦面部５１ｂとのうちの上段側の平坦面部５１ａ寄りの領域、領域Ａb3が下段側の平坦面部５１ｂ寄りの領域となるように、これらの領域Ａa3，Ａb3が設定される。
【０１８５】
図６（ａ），（ｂ）に示す第１の撮像画像においては、画像処理領域ＡＲ２のうちの、画像上暫定段差エッジラインＬep（図６（ａ）ではＬep(0)、図６（ｂ）ではＬep(n）)の下側の領域が領域Ａa3、上側の領域が領域Ａb3として設定される。
【０１８６】
なお、前記Ｓ２２の処理は、本発明における評価関数算出用領域設定手段を実現するものである。
【０１８７】
次いで、段差エッジ特定手段１２は、Ｓ２３、Ｓ２４の処理を実行する。Ｓ２３では、段差エッジ特定手段１２は、図７のＳ４の処理と同様に、上段側平面ＰＬａの平面パラメータベクトル↑ｍａを前記式（１−１）の↑ｍに代入することで決定した射影変換行列Ｐａにより、第１の撮像画像の領域Ａa3の外形枠に射影変換を施すことによって、Ａa3に対応する第２の撮像画像中の領域（以下、これを領域Ａa4という）を決定する。そして、段差エッジ特定手段１２は、Ｐａの逆変換の射影変換行列Ｐａ-1による射影変換を第２の撮像画像中の領域Ａa4の画像に施すことによって、領域Ａa4の射影変換画像を求める。
【０１８８】
また、Ｓ２４では、段差エッジ特定手段１２は、図７のＳ５の処理と同様に、下段側平面ＰＬｂの平面パラメータベクトル↑ｍｂを式（１−１）の↑ｍに代入することで決定した射影変換行列Ｐｂにより、第１の撮像画像の領域Ａb3の外形枠に射影変換を施すことによって、該領域Ａb3に対応する第２の撮像画像中の領域（以下、これを領域Ａb4という）を決定する。そして、段差エッジ特定手段１２は、Ｐｂの逆変換の射影変換行列Ｐｂ-1による射影変換を第２の撮像画像中の領域Ａb4の画像に施すことによって、領域Ａb4の射影変換画像を求める。
【０１８９】
このようにして算出される第２の撮像画像の領域Ａa4，Ａb4の各画像の射影変換画像は、暫定段差エッジラインＬｅが、実際の段差エッジに合致する場合には、それぞれ、第１の撮像画像の領域Ａa3，Ａb3のそれぞれの実際の画像に一致もしくはほぼ一致することとなる。
【０１９０】
そして、暫定段差エッジラインＬｅが、実際の段差エッジと合致していない場合には、第２の撮像画像の領域Ａa4，Ａb4の各画像の射影変換画像は、それぞれ、第１の撮像画像の領域Ａa3，Ａb3の実際の画像に対して差異を生じ、その差異の度合いが、暫定段差エッジラインＬｅと実際の段差エッジとの一致度合いに応じて変化する。
【０１９１】
そこで、段差エッジ特定手段１２は、次に、図７のＳ７〜１０と同様の処理をＳ２５〜Ｓ２７で実行し、暫定段差エッジラインＬｅと実際の段差エッジとの一致度合いに応じて値が変化する評価関数を算出する。
【０１９２】
具体的には、Ｓ２５では、段差エッジ特定手段１２は、第１の撮像画像の領域Ａa3の画像の画像値データと、第２の撮像画像の領域Ａa4の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Ｄａ２を次式（６）により算出する。
【０１９３】
また、Ｓ２６では、段差エッジ特定手段１２は、第１の撮像画像の領域Ａb3の画像の画像値データと、第２の撮像画像の領域Ａb4の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Ｄｂ２を次式（７）により算出する。
【０１９４】
これらの誤差関数Ｄａ２，Ｄｂ２はそれぞれ、本発明における第４誤差関数、第５誤差関数に相当するものである。
【０１９５】
【数２】
式（６）、（７）において、Ｉ1[u]、Ｉ2[w(u ;↑mx)]（但し、↑ｍｘ＝↑ｍａ又は↑ｍｂ）の意味は、前記式（２）〜（４）のものと同じである。また、式（６）におけるΣ、式（７）におけるΣは、それぞれ、第１の撮像画像の領域Ａa3，Ａb3における積算演算を意味する。また、式（６）のＮa2、式（７）のＮb2はそれぞれ、領域Ａa3，Ａb3の画素数である。
【０１９６】
なお、誤差関数Ｄａ２，Ｄｂ２は、前記式（２）〜（４）の誤差関数Ｄａ１，Ｄｂ１，Ｄｃ１に関して説明した場合と同様に、その他の形態のものであってもよい。例えば、Ｉ1[u]とＩ2[w(u ;↑ma)]との差の絶対値を領域Ａa3で積算したものを誤差関数Ｄａ２として使用してもよい。
【０１９７】
次いで、段差エッジ特定手段１２は、Ｓ２７の処理を実行する。このＳ２７では、段差エッジ特定手段１２は、Ｓ２５，２６でそれぞれ算出した誤差関数Ｄａ２，Ｄｂ２の値を、次式（８）により線形結合することによって、評価関数Ｅ２の値を算出する。
【０１９８】
Ｅ２＝Ｄａ２＋Ｄｂ２ ……式（８）
この場合、第１の撮像画像の領域Ａa3，Ａb3が前記したように設定されるので、上記のように算出される評価関数Ｅ２は、その値が、暫定段差エッジパラメータに依存するものとなる。そして、暫定段差エッジラインＬｅが、実際の段差エッジに合致する場合には、誤差関数Ｄａ２，Ｄｂ２の値がそれぞれ最小（≒０）となるので、評価関数Ｅ２の値も最小となる。
【０１９９】
なお、Ｄａ２，Ｄｂ２に重み係数を付して、Ｄａ２，Ｄａ１を線形結合することによって評価関数Ｅ２を算出するようにしてもよい。
【０２００】
上記の如く、評価関数Ｅ２を算出した後、段差エッジ特定手段１２は、次に、Ｓ２１において、上記評価関数Ｅ２の値を最小化する（“０”に近づける）ように、暫定段差エッジパラメータを更新する。そして、段差エッジ特定手段１２は、暫定段差エッジパラメータを更新するＳ２１の処理と、それに続くＳ２２〜Ｓ２７の処理とを繰り返す。
【０２０１】
この場合、Ｓ２１における暫定段差エッジパラメータの更新は、図７のＳ１での更新処理と同じ手法で行なわれる。
【０２０２】
そして、段差エッジ特定手段１２は、ｎ回目のＳ２７の処理で算出した評価関数Ｅ２の値と、ｎ−１回目のＳ２７の処理で算出した評価関数Ｅ２の値とを比較し、それらの値の差の絶対値が、“０”近傍の所定の設定値よりも小さくなった場合にｎ回目のＳ２２〜Ｓ２７の処理で使用した暫定段差エッジパラメータを、実際の段差エッジ５２の位置及び方向を表す最適段差エッジパラメータとして決定する。
【０２０３】
これにより、評価関数Ｅ２の値を最小化するように最適段差エッジパラメータが探索的に特定されることとなる。例えば、ｎ回目の暫定段差エッジパラメータにより規定される画像上暫定段差エッジラインＬepが図６（ｂ）に示すＬep(n)である場合、このＬep(n)に対応する暫定段差エッジパラメータが最適段差エッジパラメータとして決定される。
【０２０４】
なお、Ｓ２１での暫定段差エッジパラメータの更新処理では、Ｓ１での更新処理に関して補足した如く、種々様々な公知の探索手法を利用できる。また、ｎ回目のＳ２７の処理で算出した評価関数Ｅ２の値が、“０”近傍の所定の設定値よりも小さい場合、あるいは、暫定段差エッジパラメータを更新するＳ２１の処理と、それに続くＳ２２〜Ｓ２７の処理との繰り返し回数ｎが、所定回数に達した場合に、ｎ回目の処理で使用した暫定段差エッジパラメータを、実際の段差エッジ５２の位置及び方向の推定値を表す最適段差エッジパラメータとして決定するようにしてもよい。
【０２０５】
以上が、カメラ視線が上・下方向の場合における段差エッジ特定手段１２の処理である。
【０２０６】
以上説明した本実施形態によれば、カメラ視線が下・上方向である場合と、上・下方向である場合とのいずれの場合でも、第１の撮像画像と第２の撮像画像とから構成されるステレオ画像と、射影変換（平面射影変換）とを利用した探索手法によって、実際の段差エッジ５２の位置及び方向の推定値を表す最適段差エッジパラメータを特定できる。
【０２０７】
この場合、カメラ視線が下・上方向である場合には、段差エッジ特定手段１２は、暫定段差エッジパラメータに応じて第１の撮像画像の画像処理領域ＡＲ１内に領域Ａa1，Ａb1，Ａc1を設定し、それぞれの領域Ａa1，Ａb1，Ａc1の画像の画素値分布と、それぞれの領域Ａa1，Ａb1，Ａc1に対応する平面パラメータ（平面パラメータベクトル↑ｍａ，↑ｍｂ，↑ｍｃ）に応じた射影変換を第２の撮像画像中の対応領域Ａa2，Ａb2，Ａc2の画像に施してなる射影変換画像の画素値分布と誤差、ひいては、前記評価関数Ｅ１の値を最小化するように暫定段差エッジパラメータを更新していくことで、最適段差エッジパラメータを探索的に特定する。
【０２０８】
また、カメラ視線が上・下方向である場合には、段差エッジ特定手段１２は、暫定段差エッジパラメータに応じて第１の撮像画像の画像処理領域ＡＲ２内に領域Ａa3，Ａb3を設定し、それぞれの領域Ａa3，Ａb3の画像の画素値分布と、それぞれの領域Ａa3，Ａb3に対応する平面パラメータ（平面パラメータベクトル↑ｍａ，↑ｍｂ）に応じた射影変換を第２の撮像画像中の対応領域Ａa4，Ａb4の画像に施してなる射影変換画像の画素値分布と誤差、ひいては、前記評価関数Ｅ２の値を最小化するように暫定段差エッジパラメータを更新していくことで、最適段差エッジパラメータを探索的に特定する。
【０２０９】
これにより、カメラ視線が下・上方向である場合及び上・下方向である場合のいずれの場合においても、各撮像画像における段差エッジそのものが、その周辺の部分の画像との区別がつきにくいものとなっている場合であっても、最適段差エッジパラメータを高いロバスト性で安定に特定することができる。
【０２１０】
特に、カメラ視線が下・上方向である場合には、領域Ａc1の上側の境界線としての画像上段差エッジラインＬepとこれと反対側の境界線（下側の境界線）との間の間隔Δｃ、並びに、領域Ａc1の下側の境界線と領域Ａb1の上側の境界線との間の間隔Δｂとを前記した如く設定したことによって、階段５０の上段側の平坦面部５１ａと、下段側の平坦面部５１ｂとの間の蹴込みに相当する部分の画像が評価関数Ｅ１の値に影響を及ぼすのを抑制することができる。
【０２１１】
このため、上段側の平坦面部５１ａと下段側の平坦面部５１ｂとの間に、上段側平面ＰＬａ、起立平面ＰＬｃ、及び下段側平面ＰＬｂから逸脱した部分が存在する場合であっても、実際の段差エッジの位置及び方向を表すものとして信頼性の高い最適段差エッジパラメータを、高いロバスト性で安定に特定することができる。
【０２１２】
次に、本発明の他の態様の実施形態をいくつか説明しておく。
【０２１３】
前記実施形態では、カメラ視線が下・上方向である場合における図７のＳ３の処理で、第１の撮像画像での領域Ａb1の上側の境界線を、領域Ａc1の下側の境界線と所定の間隔Δｂを存して平行に延在するように設定したが、領域Ａb1の上側の境界線を次のように設定してもよい。すなわち、図１２に示すように、起立平面ＰＬｃ（図７のＳ２で設定した起立平面パラメータにより規定される起立平面）と、下段側平面ＰＬｂとの交線を第１の撮像画像に投影してなる直線上の線分（当該直線のうちの画像処理領域ＡＲ１内の部分）を、領域Ａb1の上側の境界線として設定してもよい。
【０２１４】
このようにすることで、前記の実施形態と同様に、階段５０の上段側の平坦面部５１ａと、下段側の平坦面部５１ｂとの間の蹴込みに相当する部分の画像が評価関数Ｅ１の値に影響を及ぼすのを抑制することができる。また、撮像カメラ３Ｒ，３Ｌと階段５０（段差部）との距離によらずに、暫定段差エッジラインＬｅが、実際の段差エッジに合致もしくはほぼ合致する場合に、領域Ａb1の上側の境界線が、下段側の平坦面部５１ｂに存在するようにすることができる。従って、暫定段差エッジラインＬｅと、実際の段差エッジとの一致度合いを、前記誤差関数Ｄｂ１に適切に反映させ、ひいては、前記評価関数Ｅ１の信頼性を適切に確保することができる。
【０２１５】
また、カメラ視線が上・下方向である場合においては、第１の撮像画像における下段側の平坦面部５１ｂの画像のうち、位置及び方向を推定しようとする実際の段差エッジに近接した部分の画像が、第２の撮像画像中には写っていない（上段側の平坦面部５１ａに陰に隠れてしまう）というようなオクルージョンが発生する場合がある。
【０２１６】
そして、このような場合に、前記実施形態の如く、第１の撮像画像おける画像上暫定段差エッジラインＬepが領域Ａa3，Ａb3の境界線となるように領域Ａa3，Ａb3を設定すると、暫定段差エッジラインＬｅが、実際の段差エッジに合致もしくはほぼ合致する状況で、領域Ａb3を下段側平面ＰＬｂの平面パラメータベクトル↑ｍｂに応じて射影変換してなる第２の撮像画像中の領域（領域Ａb3に対応する第２の撮像画像中の領域）に、上段側の平坦面部５１ａの画像が含まれてしまうこととなる。ひいては、前記式（７）により算出される誤差関数Ｄｂ２に、上記の如きオクルージョンに起因する誤差成分がノイズ成分として含まれることとなる。
【０２１７】
従って、上記の如きオクルージョンが発生する状況では、暫定段差エッジラインＬｅが、実際の段差エッジに合致もしくはほぼ合致する場合に、該オクルージョンに起因する誤差成分が、前記式（７）により算出される誤差関数Ｄｂ２に含まれるのを防止するように、前記領域Ａb3を設定することが望ましい。
【０２１８】
カメラ視線が上・下方向である場合における図１１のＳ２２の処理において、このようにオクルージョンの発生を考慮して領域Ａa3，Ａb3を設定する手法の一例を以下に図１３を参照して説明する。なお、図１３では、便宜上、第１の撮像画像における画像上暫定段差エッジラインＬep(k)は、実際の段差エッジの画像に概ね合致しているものとして示している。
【０２１９】
この手法では、段差エッジ特定手段１２は、領域Ａa3を、前記の実施形態と同様に設定する。すなわち、画像上暫定段差エッジラインＬep(k)が、領域Ａa3の上側の境界線とされ、第１の撮像画像内の画像処理領域ＡＲ２のうち、画像上暫定段差エッジラインＬep(k)から下側の領域が、領域Ａa3として設定される。
【０２２０】
一方、段差エッジ特定手段１２は、領域Ａb3を次のように設定する。まず、段差エッジ特定手段１２は、上段側平面ＰＬａの平面パラメータベクトル↑ｍａを前記式（１−１）の↑ｍに代入することで決定した射影変換行列Ｐａにより、第１の撮像画像における画像上暫定段差エッジラインＬep(k)（ｋは、暫定段差エッジパラメータの更新回数を表す）を射影変換する。これにより、図１３に示される如く、第２の撮像画像上の線分Ｌ１が得られる。
【０２２１】
また、段差エッジ特定手段１２は、下段側平面ＰＬｂの平面パラメータベクトル↑ｍｂを前記式（１−１）の↑ｍに代入することで決定した射影変換行列Ｐｂにより、第１の撮像画像における画像上暫定段差エッジラインＬep(k)を射影変換する。これにより、図１３に示される如く、第２の撮像画像上の線分Ｌ２が得られる。
【０２２２】
ここで、オクルージョンが発生している場合には、第２の撮像画像において、線分Ｌ１は、線分Ｌ２よりも上段側の平坦面部５１ａから下段側の平坦面部５１ｂに向う向き（概ね上向き）に変位した位置に存在する。
【０２２３】
そこで、段差エッジ特定手段１２は、第２の撮像画像において、線分Ｌ１が、線分Ｌ２よりも上段側の平坦面部５１ａから下段側の平坦面部５１ｂに向う向き（概ね上向き）に変位した位置に存在するか否かを判断する（以降、この判断処理をオクルージョン発生判断処理という）。そして、このオクルージョン発生判断処理の判断結果が肯定的である場合には、段差エッジ特定手段１２は、領域Ａb3の下側の境界線Ｌ３を、図１３に示す如く画像上暫定段差エッジラインＬep(k)（領域Ａa3の上側の境界線）よりも下段側の平坦面部５１ｂ寄りの位置で、Ｌep(k)と間隔を存してほぼ平行に延在するように設定する。そして、段差エッジ特定手段１２は、画像処理領域ＡＲ２のうち、上記境界線Ｌ３から上側の領域を領域Ａb3として設定する。
【０２２４】
この場合、上記境界線Ｌ３は、より具体的には次のように設定される。すなわち、第１の撮像画像における画像上暫定段差エッジラインＬep(k)を射影変換行列Ｐａにより射影変換した場合に得られた線分Ｌ１を、下段側平面パラメータ↑ｍｂに応じた射影変換行列Ｐｂの逆変換の射影変換行列Ｐｂ-1によって射影変換することで得られる線が、領域Ａb3の下側の境界線Ｌ３として設定される。
【０２２５】
また、オクルージョン発生判断処理の判断結果が否定的である場合には、段差エッジ特定手段１２は、画像上暫定段差エッジラインＬep(k)（領域Ａa3の上側の境界線）を、領域Ａb3の下側の境界線Ｌ３とし、画像処理領域ＡＲ２のうち、該境界線Ｌ３から上側の領域を領域Ａb3として設定する。
【０２２６】
以上が、オクルージョンが発生している場合の領域Ａa3，Ａb3の設定手法の一例である。
【０２２７】
カメラ視線が上・下方向である場合における図１１のＳ２２の処理において、上記の如く領域Ａa3，Ａb3を設定することによって、オクルージョンに起因する誤差成分が、前記式（７）により算出される誤差関数Ｄｂ２に含まれるのを防止することができる。このため、前記式（８）により算出される評価関数Ｅ２が、暫定段差エッジＬｅと、実際の段差エッジとの一致度合いを表すものとしての信頼性をより一層高めることができる。その結果、実際の段差エッジの位置及び方向を表すものとして最終的に特定される最適段差エッジパラメータの信頼性をより一層高めることができる。
【０２２８】
補足すると、以上説明した領域Ａa3，Ａb3の設定手法の例では、第１の撮像画像における画像上暫定段差エッジラインＬep(k)が、本発明における第１暫定段差エッジ線に相当し、第２の撮像画像上の線分Ｌ１，Ｌ２がそれぞれ、本発明における第２Ａ暫定段差エッジ線、第２Ｂ暫定段差エッジ線に相当する。
【０２２９】
また、前記実施形態では、暫定段差エッジパラメータを、前記評価関数Ｅ１又はＥ２の値に応じて初期値から逐次更新していく処理によって、最適段差エッジパラメータを特定するようにした。但し、最適段差エッジパラメータの特定を以下に説明するように行なうようにしてもよい。
【０２３０】
例えばカメラ視線が下・上方向である場合について代表的に説明すると、前記段差エッジ特定手段１２により最適段差エッジパラメータを特定する場合に、まず、実際の段差エッジの位置及び方向の推定値を表すものとして採用し得る可能性のある暫定段差エッジパラメータの複数の候補を決定しておく。
【０２３１】
この複数の暫定段差エッジパラメータの候補は、例えば次のように決定される。すなわち、各候補に対応する画像上暫定段差エッジラインＬepが、第１の撮像画像内で前記平坦面部認識手段１１によって抽出された上段側の平坦面部５１ａに対応する平坦面部投影領域Ａr1の下側の境界線（下段側の平坦面部５１ｂに対応する平坦面部投影領域Ａr2寄りの境界線）の近辺に存在し、且つ、それらの各候補に対応する画像上暫定段差エッジラインＬepの位置（具体的には、第１の撮像画像の画像処理領域ＡＲ１内での画像上暫定段差エッジラインＬepの中央点の位置（第１の撮像画像の縦方向での位置）と、方向（具体的には、第１の撮像画像の横方向に対する傾斜角度）との両方又は一方が各候補毎に互いに異なるものとなるように決定される。
【０２３２】
この場合、各候補に対応する画像上暫定段差エッジラインＬepの位置の種類数と該画像上暫定段差エッジラインＬepの方向の種類数とは、それぞれ、所定数ｍ１、ｍ２（ｍ１，ｍ２は２以上の整数）とされ、トータルｍ１×ｍ２個の暫定段差エッジパラメータの候補が決定される。なお、各候補に対応する画像上暫定段差エッジラインＬepのｍ１個の位置は、例えば、上段側の平坦面部５１ａに対応する平坦面部投影領域Ａr1の下側の境界線の近辺の所定範囲内で第１の撮像画像の縦方向に微小量づつ、異なる位置となるように決定される。また、各候補に対応する画像上暫定段差エッジラインＬepのｍ２個の方向は、例えば、第１の撮像画像の横方向に対する傾斜角度（鋭角側の傾斜角度）が０°近辺の所定範囲内で微小量づつ、異なる角度となるように決定される。
【０２３３】
そして、上記のように決定した暫定段差エッジパラメータの各候補毎に、前記した図７のＳ２〜Ｓ１０と同じ処理を実行することによって評価関数Ｅ１の値を算出し、暫定段差エッジパラメータの候補のうち、算出した評価関数Ｅ１の値が最小となる候補を、最適段差エッジパラメータとして特定する。
【０２３４】
カメラ視線が上・下方向である場合においても、上記と同様に、暫定段差エッジパラメータの複数の候補の中から、最適段差エッジパラメータを特定することができる。この場合は、暫定段差エッジパラメータの複数の候補の設定の仕方は、カメラ視線が下・上方向である場合と同じよい。そして、各候補毎の評価関数Ｅ２は、前記した図１１のＳ２２〜Ｓ２７と同じ処理を実行することによって算出するようにすればよい。
【０２３５】
また、以上説明した実施形態では、カメラ視線が下・上方向である場合に、評価関数Ｅ１を、３つの誤差関数Ｄａ１，Ｄｂ１，Ｄｃ１を線形結合することによって構成したが、誤差関数Ｄｂ１を省略し、誤差関数Ｄａ１，Ｄｃ１の２つの誤差関数を線形結合することによって評価関数Ｅ１を構成するようにしてもよい。この場合には、第１の撮像画像における領域Ａb1と、これに対応する第２の撮像画像の領域Ａb2の設定は不要である。
【０２３６】
また、前記実施形態では、カメラ視線が下・上方向である場合に、上段側平面ＰＬａに対する前記起立平面ＰＬｃの傾斜角度を既定値とすることで、段差エッジ特定手段１２の演算処理負負荷が軽減されることとなるが、段差エッジパラメータに加えて、該傾斜角度も暫定的に仮決定するようにして、前記評価関数Ｅ１の値を最小化するように、段差エッジパラメータと該傾斜角度との両方を探索的に決定するようにしてもよい。
【０２３７】
また、前記実施形態では、カメラ３Ｒの撮像画像を第１の撮像画像として使用する場合を例にとって説明したが、カメラ３Ｌの撮像画像を第１の撮像画像として使用してもよいことはもちろんである。
【０２３８】
また、前記実施形態では、段差部としての階段５０の段差エッジの位置及び方向を推定する場合を主要例として説明したが、階段５０以外の段差部、例えば、床に設置される踏み台等の設置物により構成される段差部の段差エッジであっても、その位置及び方向を前記実施形態と同様に推定することができる。
【０２３９】
また、前記実施形態では、カメラ３Ｒ，３Ｌをロボット１に搭載した場合を例にとって説明したが、カメラ３Ｒ，３Ｌは、階段等の段差部を移動可能なロボット１以外の移動体に搭載したものでもあってもよい。
【符号の説明】
【０２４０】
３Ｒ，３Ｌ…カメラ（撮像手段）、１２…段差エッジ特定手段、５０…階段（段差部）、５１ａ…上段側の平坦面部、５１ｂ…下段側の平坦面部、５２…段差エッジ、Ｓ３，Ｓ２２…評価関数算出用領域設定手段、Ａa1…第１の領域、Ａb1…第２の領域、Ａc1…第３の領域、Ａa3…第４の領域、Ａb3…第５の領域、Ｅ１，Ｅ２…評価関数、Ｄａ１…第１誤差関数、Ｄｂ１…第２誤差関数、Ｄｃ１…第３誤差関数、Ｄａ２…第４誤差関数、Ｄｂ２…第５誤差関数。
【特許請求の範囲】
【請求項１】
第１の撮像画像及び第２の撮像画像から構成されるステレオ画像を撮像するための撮像手段を用い、下段側の平坦面部と、該下段側の平坦面部の一段上側の上段側の平坦面部との組を少なくとも一組有する段差部を、その下段側が手前側、上段側が奥側となる視線で前記撮像手段により撮像し、その撮像により得られた該段差部のステレオ画像を基に、前記上段側の平坦面部の先端側エッジである段差エッジの３次元空間中の位置及び方向を推定する段差エッジ推定装置であって、
前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値を、暫定的に仮決定した段差エッジパラメータと、前記上段側の平坦面部を包含する上段側平面の位置及び姿勢を表すものとして事前に特定された上段側平面パラメータと、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジを含むと共に前記上段側平面に対して起立した姿勢となる起立平面の位置及び姿勢を表すものとして、前記仮決定した段差エッジパラメータ及び前記上段側平面パラメータに応じて設定した起立平面パラメータとを用いて算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する段差エッジ特定手段を備え、
前記評価関数は、
前記第１の撮像画像中に設定した第１の領域における画像の画素値分布と、前記第２の撮像画像中の前記第１の領域に対応する領域における画像を前記起立平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第１誤差関数と、
前記第１の撮像画像中に設定した第２の領域における画像の画素値分布と、前記第２の撮像画像中の前記第２の領域に対応する領域における画像を前記上段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第２誤差関数とを線形結合して構成された評価関数であり、
前記段差エッジ特定手段は、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第１の撮像画像中において前記第１の領域及び第２の領域の境界線となると共に、前記第１の領域が、前記第１の撮像画像中において前記上段側の平坦面部とその一段下側の下段側の平坦面部との間の部分である上下段間部分と前記上段側の平坦面部とのうちの上下段間部分寄りの領域、前記第２の領域が前記上下段間部分と前記上段側の平坦面部とのうちの上段側の平坦面部寄りの領域となるように該第１の領域及び第２の領域を設定する評価関数算出用領域設定手段を含むことを特徴とする段差エッジ推定装置。
【請求項２】
請求項１記載の段差エッジ推定装置において、
前記段差エッジ特定手段は、前記上段側平面又は水平面に対する前記起立平面の傾斜角度を、事前に設定された所定の角度に規定して前記起立平面パラメータを設定することを特徴とする段差エッジ推定装置。
【請求項３】
請求項１又は２記載の段差エッジ推定装置において、
前記評価関数算出用領域設定手段は、前記第１の撮像画像中において、前記第１の領域の前記第２の領域と反対側の境界線が、前記暫定的な段差エッジと所定の間隔を存して平行に延在するように該第１の領域を設定する手段を含んでおり、該所定の間隔は、前記暫定的な段差エッジが実際の段差エッジに一致する場合に、前記第１の領域の前記第２の領域と反対側の境界線が、前記上下段間部分において前記上段側の平坦面部とその一段下側の下段側の平坦面部とのうちの上段側の平坦面部寄りの境界線となるように設定された間隔であることを特徴とする段差エッジ推定装置。
【請求項４】
請求項１〜３のいずれか１項に記載の段差エッジ推定装置において、
前記段差エッジ特定手段は、暫定的に仮決定する前記段差エッジパラメータを、その初期値から逐次更新していくことによって、前記最適段差エッジパラメータを特定するように構成されると共に、前記段差エッジパラメータの初期値により規定される暫定的な段差エッジが、前記上段側の平坦面部のうち、平面であることが事前に特定されている領域を通るように該初期値を設定する手段を含むことを特徴とする段差エッジの推定装置。
【請求項５】
請求項４記載の段差エッジ推定装置において、
前記撮像手段は、前記段差部を移動する移動体に、該移動体の進行方向前方を撮像するように搭載されており、前記段差エッジパラメータの初期値を設定する手段は、該初期値により規定される暫定的な段差エッジの方向が前記移動体の進行方向に対して交差する方向になるように該初期値を設定することを特徴とする段差エッジの推定装置。
【請求項６】
請求項１〜５のいずれか１項に記載の段差エッジ推定装置において、
前記評価関数は、前記第１の撮像画像中に設定した第３の領域における画像の画素値分布と、前記第２の撮像画像中の前記第３の領域に対応する領域における画像を、前記上段側の平坦面部の一段下側の下段側の平坦面部を包含する下段側平面の位置及び姿勢を表すものとして事前に特定された下段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第３誤差関数を、前記第１誤差関数と第２誤差関数とにさらに線形結合して構成された評価関数であり、
前記評価関数算出用領域設定手段は、前記第３の領域が、前記第１の撮像画像中において前記第１の領域よりも前記下段側の平坦面部寄りの位置で該下段側の平坦面部の少なくとも一部分を含むと共に、該第３の領域の前記第１の領域側の境界線の該第１の撮像画像中での位置及び方向を、該第１の撮像画像中での前記暫定的な段差エッジの位置及び方向に応じて規定するように該第３の領域を設定する手段を含むことを特徴とする段差エッジ推定装置。
【請求項７】
請求項６記載の段差エッジ推定装置において、
前記第３の領域を設定する手段は、該第３の領域の前記第１の領域側の境界線が、前記暫定的な段差エッジと平行に延在するように該第３の領域を設定することを特徴とする段差エッジ推定装置。
【請求項８】
請求項６又は７記載の段差エッジ推定装置において、
前記第３の領域を設定する手段は、前記第１の撮像画像において第１の領域と第３の領域との間に所定の間隔を存するように該第３の領域を設定するものであり、該所定の間隔は、前記第３の領域の前記第１の領域側の境界線が、前記第１の撮像画像において前記下段側の平坦面部内に存在するように設定された間隔であることを特徴とする段差エッジ推定装置。
【請求項９】
請求項６記載の段差エッジ推定装置において、
前記第３の領域を設定する手段は、前記第３の領域の前記第１の領域側の境界線が、前記仮決定した段差エッジパラメータ及び前記上段側平面パラメータに応じて設定された前記起立平面パラメータに対応する前記起立平面と前記下段側平面との交線を前記第１の撮像画像に投影した直線上の線分になるように、該第３の領域の第１の領域側の境界線を設定することを特徴とする段差エッジ推定装置。
【請求項１０】
第１の撮像画像及び第２の撮像画像から構成されるステレオ画像を撮像するための撮像手段を用い、下段側の平坦面部と、該下段側の平坦面部の一段上側の上段側の平坦面部との組を少なくとも一組有する段差部を、その上段側が手前側、下段側を奥側となる視線で前記撮像手段により撮像し、その撮像により得られた該段差部のステレオ画像を基に、前記上段側の平坦面部の先端側エッジである段差エッジの３次元空間中の位置及び方向を推定する段差エッジ推定装置であって、
前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値を、暫定的に仮決定した段差エッジパラメータと、前記上段側の平坦面部を包含する上段側平面の位置及び姿勢を表すものとして事前に特定された上段側平面パラメータと、前記上段側の平坦面部の一段下側の下段側平坦面部を包含する下段側平面の位置及び姿勢を表すものとして事前に特定された下段側平面パラメータとを用いて算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する段差エッジ特定手段を備え、
前記評価関数は、
前記第１の撮像画像中に設定した第４の領域における画像の画素値分布と、前記第２の撮像画像中の前記第４の領域に対応する領域における画像を前記上段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第４誤差関数と、
前記第１の撮像画像中に設定した第５の領域における画像の画素値分布と、前記第２の撮像画像中の前記第５の領域に対応する領域における画像を前記下段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第５誤差関数とを線形結合して構成された評価関数であり、
前記段差エッジ特定手段は、前記第１の撮像画像中において、前記第４の領域が、前記上段側の平坦面部と前記下段側の平坦面部とのうちの上段側の平坦面部寄りの領域、前記第５の領域が前記上段側の平坦面部と前記下段側の平坦面部とのうちの下段側の平坦面部寄りの領域となると共に、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第１の撮像画像中において前記第４の領域の前記第５の領域側の境界線となり、且つ、前記第５の領域の前記第４の領域側の境界線が、前記第４の領域の前記第５の領域側の境界線に一致するか、又は、前記第４の領域の前記第５の領域側の境界線と間隔を存して当該第５の領域側の境界線と平行に延在するように該第４の領域及び第５の領域を設定する評価関数算出用領域設定手段を含むことを特徴とする段差エッジ推定装置。
【請求項１１】
請求項１０記載の段差エッジ推定装置において、
前記評価関数算出用領域設定手段は、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジを前記第１の撮像画像に投影してなる線である第１暫定段差エッジ線を、前記上段側平面パラメータに応じて第１の撮像画像側から第２の撮像画像側に射影変換した場合に得られる第２Ａ暫定段差エッジ線と、前記第１暫定段差エッジ線を、前記下段側平面パラメータに応じて第１の撮像画像側から第２の撮像画像側に射影変換した場合に得られる第２Ｂ暫定段差エッジ線との第２の撮像画像中での位置関係を比較し、該第２の撮像画像での第２Ａ暫定段差エッジ線の位置が、前記第２Ｂ暫定段差エッジ線よりも、前記上段側の平坦面部から下段側の平坦面部に向う向きに変位した位置であるか否かを判断する手段と、該判断結果が肯定的である場合には、前記第５の領域の前記第４の領域側の境界線が、前記第２Ａ暫定段差エッジ線を、前記下段側平面パラメータに応じて第２の撮像画像側から第１の撮像画像側に射影変換した場合に得られる線上に存在するように前記第５の領域を設定し、前記判断結果が否定的である場合には、前記第５の領域の前記第４の領域側の境界線が、前記第１暫定段差エッジ線上に存在するように前記第５の領域を設定する手段とを備えることを特徴とする段差エッジ推定装置。
【請求項１】
第１の撮像画像及び第２の撮像画像から構成されるステレオ画像を撮像するための撮像手段を用い、下段側の平坦面部と、該下段側の平坦面部の一段上側の上段側の平坦面部との組を少なくとも一組有する段差部を、その下段側が手前側、上段側が奥側となる視線で前記撮像手段により撮像し、その撮像により得られた該段差部のステレオ画像を基に、前記上段側の平坦面部の先端側エッジである段差エッジの３次元空間中の位置及び方向を推定する段差エッジ推定装置であって、
前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値を、暫定的に仮決定した段差エッジパラメータと、前記上段側の平坦面部を包含する上段側平面の位置及び姿勢を表すものとして事前に特定された上段側平面パラメータと、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジを含むと共に前記上段側平面に対して起立した姿勢となる起立平面の位置及び姿勢を表すものとして、前記仮決定した段差エッジパラメータ及び前記上段側平面パラメータに応じて設定した起立平面パラメータとを用いて算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する段差エッジ特定手段を備え、
前記評価関数は、
前記第１の撮像画像中に設定した第１の領域における画像の画素値分布と、前記第２の撮像画像中の前記第１の領域に対応する領域における画像を前記起立平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第１誤差関数と、
前記第１の撮像画像中に設定した第２の領域における画像の画素値分布と、前記第２の撮像画像中の前記第２の領域に対応する領域における画像を前記上段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第２誤差関数とを線形結合して構成された評価関数であり、
前記段差エッジ特定手段は、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第１の撮像画像中において前記第１の領域及び第２の領域の境界線となると共に、前記第１の領域が、前記第１の撮像画像中において前記上段側の平坦面部とその一段下側の下段側の平坦面部との間の部分である上下段間部分と前記上段側の平坦面部とのうちの上下段間部分寄りの領域、前記第２の領域が前記上下段間部分と前記上段側の平坦面部とのうちの上段側の平坦面部寄りの領域となるように該第１の領域及び第２の領域を設定する評価関数算出用領域設定手段を含むことを特徴とする段差エッジ推定装置。
【請求項２】
請求項１記載の段差エッジ推定装置において、
前記段差エッジ特定手段は、前記上段側平面又は水平面に対する前記起立平面の傾斜角度を、事前に設定された所定の角度に規定して前記起立平面パラメータを設定することを特徴とする段差エッジ推定装置。
【請求項３】
請求項１又は２記載の段差エッジ推定装置において、
前記評価関数算出用領域設定手段は、前記第１の撮像画像中において、前記第１の領域の前記第２の領域と反対側の境界線が、前記暫定的な段差エッジと所定の間隔を存して平行に延在するように該第１の領域を設定する手段を含んでおり、該所定の間隔は、前記暫定的な段差エッジが実際の段差エッジに一致する場合に、前記第１の領域の前記第２の領域と反対側の境界線が、前記上下段間部分において前記上段側の平坦面部とその一段下側の下段側の平坦面部とのうちの上段側の平坦面部寄りの境界線となるように設定された間隔であることを特徴とする段差エッジ推定装置。
【請求項４】
請求項１〜３のいずれか１項に記載の段差エッジ推定装置において、
前記段差エッジ特定手段は、暫定的に仮決定する前記段差エッジパラメータを、その初期値から逐次更新していくことによって、前記最適段差エッジパラメータを特定するように構成されると共に、前記段差エッジパラメータの初期値により規定される暫定的な段差エッジが、前記上段側の平坦面部のうち、平面であることが事前に特定されている領域を通るように該初期値を設定する手段を含むことを特徴とする段差エッジの推定装置。
【請求項５】
請求項４記載の段差エッジ推定装置において、
前記撮像手段は、前記段差部を移動する移動体に、該移動体の進行方向前方を撮像するように搭載されており、前記段差エッジパラメータの初期値を設定する手段は、該初期値により規定される暫定的な段差エッジの方向が前記移動体の進行方向に対して交差する方向になるように該初期値を設定することを特徴とする段差エッジの推定装置。
【請求項６】
請求項１〜５のいずれか１項に記載の段差エッジ推定装置において、
前記評価関数は、前記第１の撮像画像中に設定した第３の領域における画像の画素値分布と、前記第２の撮像画像中の前記第３の領域に対応する領域における画像を、前記上段側の平坦面部の一段下側の下段側の平坦面部を包含する下段側平面の位置及び姿勢を表すものとして事前に特定された下段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第３誤差関数を、前記第１誤差関数と第２誤差関数とにさらに線形結合して構成された評価関数であり、
前記評価関数算出用領域設定手段は、前記第３の領域が、前記第１の撮像画像中において前記第１の領域よりも前記下段側の平坦面部寄りの位置で該下段側の平坦面部の少なくとも一部分を含むと共に、該第３の領域の前記第１の領域側の境界線の該第１の撮像画像中での位置及び方向を、該第１の撮像画像中での前記暫定的な段差エッジの位置及び方向に応じて規定するように該第３の領域を設定する手段を含むことを特徴とする段差エッジ推定装置。
【請求項７】
請求項６記載の段差エッジ推定装置において、
前記第３の領域を設定する手段は、該第３の領域の前記第１の領域側の境界線が、前記暫定的な段差エッジと平行に延在するように該第３の領域を設定することを特徴とする段差エッジ推定装置。
【請求項８】
請求項６又は７記載の段差エッジ推定装置において、
前記第３の領域を設定する手段は、前記第１の撮像画像において第１の領域と第３の領域との間に所定の間隔を存するように該第３の領域を設定するものであり、該所定の間隔は、前記第３の領域の前記第１の領域側の境界線が、前記第１の撮像画像において前記下段側の平坦面部内に存在するように設定された間隔であることを特徴とする段差エッジ推定装置。
【請求項９】
請求項６記載の段差エッジ推定装置において、
前記第３の領域を設定する手段は、前記第３の領域の前記第１の領域側の境界線が、前記仮決定した段差エッジパラメータ及び前記上段側平面パラメータに応じて設定された前記起立平面パラメータに対応する前記起立平面と前記下段側平面との交線を前記第１の撮像画像に投影した直線上の線分になるように、該第３の領域の第１の領域側の境界線を設定することを特徴とする段差エッジ推定装置。
【請求項１０】
第１の撮像画像及び第２の撮像画像から構成されるステレオ画像を撮像するための撮像手段を用い、下段側の平坦面部と、該下段側の平坦面部の一段上側の上段側の平坦面部との組を少なくとも一組有する段差部を、その上段側が手前側、下段側を奥側となる視線で前記撮像手段により撮像し、その撮像により得られた該段差部のステレオ画像を基に、前記上段側の平坦面部の先端側エッジである段差エッジの３次元空間中の位置及び方向を推定する段差エッジ推定装置であって、
前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値を、暫定的に仮決定した段差エッジパラメータと、前記上段側の平坦面部を包含する上段側平面の位置及び姿勢を表すものとして事前に特定された上段側平面パラメータと、前記上段側の平坦面部の一段下側の下段側平坦面部を包含する下段側平面の位置及び姿勢を表すものとして事前に特定された下段側平面パラメータとを用いて算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する段差エッジ特定手段を備え、
前記評価関数は、
前記第１の撮像画像中に設定した第４の領域における画像の画素値分布と、前記第２の撮像画像中の前記第４の領域に対応する領域における画像を前記上段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第４誤差関数と、
前記第１の撮像画像中に設定した第５の領域における画像の画素値分布と、前記第２の撮像画像中の前記第５の領域に対応する領域における画像を前記下段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第５誤差関数とを線形結合して構成された評価関数であり、
前記段差エッジ特定手段は、前記第１の撮像画像中において、前記第４の領域が、前記上段側の平坦面部と前記下段側の平坦面部とのうちの上段側の平坦面部寄りの領域、前記第５の領域が前記上段側の平坦面部と前記下段側の平坦面部とのうちの下段側の平坦面部寄りの領域となると共に、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第１の撮像画像中において前記第４の領域の前記第５の領域側の境界線となり、且つ、前記第５の領域の前記第４の領域側の境界線が、前記第４の領域の前記第５の領域側の境界線に一致するか、又は、前記第４の領域の前記第５の領域側の境界線と間隔を存して当該第５の領域側の境界線と平行に延在するように該第４の領域及び第５の領域を設定する評価関数算出用領域設定手段を含むことを特徴とする段差エッジ推定装置。
【請求項１１】
請求項１０記載の段差エッジ推定装置において、
前記評価関数算出用領域設定手段は、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジを前記第１の撮像画像に投影してなる線である第１暫定段差エッジ線を、前記上段側平面パラメータに応じて第１の撮像画像側から第２の撮像画像側に射影変換した場合に得られる第２Ａ暫定段差エッジ線と、前記第１暫定段差エッジ線を、前記下段側平面パラメータに応じて第１の撮像画像側から第２の撮像画像側に射影変換した場合に得られる第２Ｂ暫定段差エッジ線との第２の撮像画像中での位置関係を比較し、該第２の撮像画像での第２Ａ暫定段差エッジ線の位置が、前記第２Ｂ暫定段差エッジ線よりも、前記上段側の平坦面部から下段側の平坦面部に向う向きに変位した位置であるか否かを判断する手段と、該判断結果が肯定的である場合には、前記第５の領域の前記第４の領域側の境界線が、前記第２Ａ暫定段差エッジ線を、前記下段側平面パラメータに応じて第２の撮像画像側から第１の撮像画像側に射影変換した場合に得られる線上に存在するように前記第５の領域を設定し、前記判断結果が否定的である場合には、前記第５の領域の前記第４の領域側の境界線が、前記第１暫定段差エッジ線上に存在するように前記第５の領域を設定する手段とを備えることを特徴とする段差エッジ推定装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【公開番号】特開２０１２−１７７６０８（Ｐ２０１２−１７７６０８Ａ）
【公開日】平成２４年９月１３日（２０１２．９．１３）
【国際特許分類】
【出願番号】特願２０１１−４０４８７（Ｐ２０１１−４０４８７）
【出願日】平成２３年２月２５日（２０１１．２．２５）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【出願人】（３０４０２１４１７）国立大学法人東京工業大学 (1,821)
【Ｆターム（参考）】
【公開日】平成２４年９月１３日（２０１２．９．１３）
【国際特許分類】
【出願日】平成２３年２月２５日（２０１１．２．２５）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【出願人】（３０４０２１４１７）国立大学法人東京工業大学 (1,821)
【Ｆターム（参考）】
[ Back to top ]