ところで、2足移動ロボット等の脚式移動ロボットに階段等の段差部の昇降動作を行なわせる場合、ロボットの脚の足平部を、段差部の上段側又は下段側の平坦面部(以下、踏面ということがある)の先端部のエッジ(特許文献1で言うところのフロントエッジ)に載せて、該足平部の一部を段差部の踏面からはみ出させることが必要もしくは好ましい場合がある。
例えば、階段の踏面の奥行き寸法が、ロボットの足平部の大きさに比して狭い場合には、該足平部の全体を踏面に載せることが不可能な場合がある。あるいは、ロボットの足平部の全体を踏面に載せることが可能であっても、該足平部を振り出すときに、該足平部が階段の蹴り込み等と干渉するのを避けるために、ロボットの足平部の一部を踏面からはみ出させることが好ましい場合もある。
そして、このような場合には、ロボットの足平部の目標とする着地位置を適切に決定するために、階段等の段差部の先端部のエッジの位置及びその方向を精度よく認識する必要がある。
しかるに、前記特許文献1に見られる如き従来の手法では、階段などの段差部の踏面の先端部のエッジを検出することができない場合が多々ある。
すなわち、ステレオビジョンシステムによって得られるステレオ画像から認識される距離データは、撮像画像のテクスチャ等の影響を受けやすいので、段差部の踏面の先端部のエッジ付近での信頼性の高い距離データを得ることができない場合も多々ある。そして、このような場合には、段差部の踏面の先端部のエッジ付近は、平面部として認識できないか、もしくは、平面部から除外されることとなる。その結果、前記多角形は、段差部の踏面の先端部のエッジと異なる境界の形状を有するものとなり、該多角形から認識されるフロントエッジは、段差部の踏面の先端部の実際のエッジとの誤差が大きなものとなりやすい。
従って、前記特許文献1に見られる如き従来の手法では、階段などの段差部の踏面の先端部のエッジを検出することができない場合が多々ある。
本発明は、かかる背景に鑑みてなされたものであり、撮像手段により取得されるステレオ画像を利用して、階段等の段差部の平坦面部の先端部のエッジの位置及び方向を精度良く推定することを可能とする装置を提供することを目的とする。
ここで、本発明における用語について補足しておく。本発明においては、「段差部」は、上段側の平坦面部とその一段下側に存在する下段側の平坦面部との組が一組もしくは複数組含まれるものである。この「段差部」は、階段を含むものであるが、階段だけを意味するものではない。例えば、床面に設置される踏み台もしくは任意の設置物により構成される段差部も含まれる。
また、段差部の平坦面部の「先端側エッジ」又は「段差エッジ」というのは、段差部をその下段側が手前側、上段側が奥側となる視線で見た場合において、各段の平坦面部(階段で言うところの踏面)の外形の境界線のうち、手前側で横方向(水平方向もしくはそれに近い方向)に直線状に延在する境界線を意味する。
あるいは、換言すれば、段差部の平坦面部の「先端側エッジ」又は「段差エッジ」というのは、段差部をその上段側が手前側、下段側が奥側となる視線で見た場合において、各段の平坦面部の外形の境界線のうち、奥側で横方向に直線状に延在する境界線を意味する。
以上を前提として、本発明を以下に説明する。
本発明は、上記目的を達成するために、第1の撮像画像及び第2の撮像画像から構成されるステレオ画像を撮像するための撮像手段を用い、下段側の平坦面部と、該下段側の平坦面部の一段上側の上段側の平坦面部との組を少なくとも一組有する段差部を、その下段側が手前側、上段側が奥側となる視線で前記撮像手段により撮像し、その撮像により得られた該段差部のステレオ画像を基に、前記上段側の平坦面部の先端側エッジである段差エッジの3次元空間中の位置及び方向を推定する段差エッジ推定装置であって、
前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値を、暫定的に仮決定した段差エッジパラメータと、前記上段側の平坦面部を包含する上段側平面の位置及び姿勢を表すものとして事前に特定された上段側平面パラメータと、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジを含むと共に前記上段側平面に対して起立した姿勢となる起立平面の位置及び姿勢を表すものとして、前記仮決定した段差エッジパラメータ及び前記上段側平面パラメータに応じて設定した起立平面パラメータとを用いて算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する段差エッジ特定手段を備え、
前記評価関数は、
前記第1の撮像画像中に設定した第1の領域における画像の画素値分布と、前記第2の撮像画像中の前記第1の領域に対応する領域における画像を前記起立平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第1誤差関数と、
前記第1の撮像画像中に設定した第2の領域における画像の画素値分布と、前記第2の撮像画像中の前記第2の領域に対応する領域における画像を前記上段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第2誤差関数とを線形結合して構成された評価関数であり、
前記段差エッジ特定手段は、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第1の撮像画像中において前記第1の領域及び第2の領域の境界線となると共に、前記第1の領域が、前記第1の撮像画像中において前記上段側の平坦面部とその一段下側の下段側の平坦面部との間の部分である上下段間部分と前記上段側の平坦面部とのうちの上下段間部分寄りの領域、前記第2の領域が前記上下段間部分と前記上段側の平坦面部とのうちの上段側の平坦面部寄りの領域となるように該第1の領域及び第2の領域を設定する評価関数算出用領域設定手段を含むことを特徴とする(第1発明)。
ここで、一般に、前記段差部の上段側の平坦面部の先端部には、階段における段鼻の先端面の如く、上段側の平坦面部からこれに対して起立した姿勢で垂下するほぼ平坦な起立面が存在し、この起立面を包含する起立平面と上段側の平坦面部を包含する上段側平面との交線が前記段差エッジに相当するものである。そして、前記段差部をその下段側が手前側、上段側が奥側となる視線で前記撮像手段により撮像した場合には、該起立面が第1の撮像画像及び第2の撮像画像に投影されることとなる。
また、各撮像画像における上段側の平坦面部や下段側の平坦面部は比較的広い面積を有することから、それらの平坦面部の撮像手段に対する位置及び姿勢は、距離センサや前記ステレオ画像等を利用した公知の手法によって、比較的容易に特定しておくことができる。
さらに、前記段差エッジの位置及び方向を、暫定的に、ある適当な位置及び方向に仮決定し、その仮決定した位置及び方向を有する段差エッジが上段側平面との交線となるように起立平面を設定した場合に、その起立平面が実際の段差部における前記起立面を包含する平面であるか否かは、前記第1の撮像画像及び第2の撮像画像から構成されるステレオ画像と射影変換(平面射影変換)とを利用して評価することが可能である。
前記第1発明は、これらの事項を考慮して構築されたものである。
この第1発明によれば、前記段差エッジ特定手段は、前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値の大きさを最小化するように、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する。
この場合、上記評価関数は、前記第1誤差関数と第2誤差関数とを線形結合して構成される。そして、この第1誤差関数は、前記第1の撮像画像中に設定した第1の領域における画像の画素値分布と、前記第2の撮像画像中の前記第1の領域に対応する領域における画像を、暫定的に仮決定した段差エッジパラメータと事前に特定された上段側平面パラメータとに応じて設定した前記起立平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す関数である。
また、第2誤差関数は、前記第1の撮像画像中に設定した第2の領域における画像の画素値分布と、前記第2の撮像画像中の前記第2の領域に対応する領域における画像を前記上段側平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す関数である。
さらに、前記第1の領域と第2の領域とは、前記した如く、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジを前記第1の撮像画像中における両領域の境界線として設定される領域である。
このため、前記評価関数の値は、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジの位置及び方向の、実際の段差エッジの位置及び方向に対する誤差の度合いを表すものとなり、暫定的に仮決定した段差エッジパラメータにより規定される段差エッジの位置及び方向が、実際の段差エッジの位置及び方向に合致していれば、前記評価関数の値は、最小となるものである。
従って、暫定的に仮決定した段差エッジパラメータに対して前記評価関数を算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、前記最適段差エッジパラメータを特定することによって、該最適段差エッジパラメータを、実際の段差エッジの位置及び姿勢の推定値を表すものとして得られることとなる。
なお、前記評価関数の値の大きさを最小化するように、前記最適段差エッジパラメータを特定するということは、評価関数の値が最小値、もしくは、その近似値(当該最小値との差が所要の許容範囲(段差エッジの位置及び方向の推定精度を目標とする精度に確保するために要求される許容範囲)内に収まる値)となると見なし得る段差エッジパラメータを、最適段差エッジパラメータとして特定することを意味する。従って、特定した最適段差エッジパラメータに対して算出される前記評価関数の値が厳密に該評価関数の最小値である必要はない。
かかる第1発明によれば、前記段差エッジが、前記上段側平面と起立平面との交線となることを前提とすることによって、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジの位置及び方向の、実際の段差エッジの位置及び方向に対する誤差の度合いを、前記した如く構成した評価関数の値によって適正に評価することができる。このため、撮像手段により取得される第1の撮像画像及び第2の撮像画像から構成されるステレオ画像を利用して、階段等の段差部の平坦面部の先端部のエッジの位置及び方向を精度良く推定することが可能となる。
なお、第1誤差関数の代表的な例としては、前記第1の撮像画像中の第1の領域における画像の各画素値(輝度値など)と、前記第2の撮像画像中の前記第1の領域に対応する領域における画像の射影変換画像の各画素値との偏差の絶対値又は二乗値の積算値、あるいは、該絶対値又は二乗値の平均値が挙げられる。第2誤差関数についても同様である。
前記第1発明では、前記段差エッジ特定手段は、前記上段側平面又は水平面に対する前記起立平面の傾斜角度を、事前に設定された所定の角度に規定して前記起立平面パラメータを設定することが好ましい(第2発明)。
この第2発明によれば、前記上段側平面又は水平面に対する前記起立平面の傾斜角度が既定値とされることとなるので、暫定的に仮決定した段差エッジパラメータと、前記上段側平面パラメータとから前記起立平面パラメータを設定できる。このため、未知数が、段差エッジパラメータだけとなり、前記最適段差エッジパラメータの探索的な特定処理が容易になる。
但し、第1発明においては、前記起立平面の傾斜角度と、段差エッジパラメータとの組を未知数として、該傾斜角度と最適段差エッジパラメータとを特定するようにすることも可能である。
また、第1発明又は第2発明においては、前記評価関数算出用領域設定手段は、前記第1の撮像画像中において、前記第1の領域の前記第2の領域と反対側の境界線が、前記暫定的な段差エッジと所定の間隔を存して平行に延在するように該第1の領域を設定する手段を含んでおり、該所定の間隔は、前記暫定的な段差エッジが実際の段差エッジに一致する場合に、前記第1の領域の前記第2の領域と反対側の境界線が、前記上下段間部分において前記上段側の平坦面部とその一段下側の下段側の平坦面部とのうちの上段側の平坦面部寄りの境界線となるように設定された間隔であることが好ましい(第3発明)。
すなわち、一般的な階段等の段差部において、上段側の平坦面部の先端部で、該平坦面部からこれに対して起立した姿勢で垂下する起立面は、多くの場合、下段側の平坦面部まで同一平面上で連なっているわけではなく、上段側の平坦面部寄りの箇所で、高さ方向に比較的小さな幅(上段側の平坦面部を構成する板部材の厚さ分に相当する幅)を有する面となっている場合が多い。そして、上段側の平坦面部と下段側の平坦面部との間の箇所(階段の蹴込みに相当する箇所)のうちの、下段側の平坦面部寄りの箇所には、起立面が存在しないか、又は、上段側の平坦面部に連なる起立面よりも奥側に存在する面となっている場合が多い。
そこで、第3発明では、上記の如く、第1の領域を設定するようにした。これにより、特定の種類に限られない一般的な形状の種々様々な段差部に対して、前記暫定的な段差エッジが実際の段差エッジに一致する場合における第1の領域の全体又は大部分が、段差部における前記起立平面上に存在する部分の画像となるように、前記第1の領域を設定できることとなる。この結果、一般的な形状の種々様々な段差部に対して、前記評価関数の信頼性(暫定的な段差エッジの位置及び方向の誤差を表すものとしての信頼性)を高め、ひいては、実際の段差エッジの位置及び方向の推定の信頼性を高めることができると共に、その推定処理の汎用性を高めることができる。
前記第1〜第3の発明において、前記最適段差エッジパラメータを特定するための代表的な手法としては、次のような2つの手法を用いることができる。その第1の手法では、前記段差エッジ特定手段は、暫定的に仮決定する前記段差エッジパラメータの候補としてあらかじめ定められた複数の段差エッジパラメータ候補のうち、当該候補に対応して算出される前記評価関数の値が最小となる段差エッジパラメータ候補を前記最適段差エッジパラメータとして特定するように構成される。
また、第2の手法では、前記段差エッジ特定手段は、暫定的に仮決定する前記段差エッジパラメータを、その初期値から逐次更新していくことによって、前記最適段差エッジパラメータを特定するように構成される。なお、暫定的に仮決定する前記段差エッジパラメータを、その初期値から逐次更新していくということは、より詳しくは、段差エッジパタメータを暫定的に仮決定し、少なくともその仮決定した段差エッジパラメータに対応して算出される評価関数の値に応じて、新たな暫定的な段差エッジパラメータを仮決定するという処理を繰り返すことを意味する。
前記第1〜第3の発明においては、上記第1の手法及び第2の手法のいずれの手法を採用することもできる。そして、特に上記第2の手法を採用する場合には、前記初期値は、次のように設定することが好適である。すなわち、前記第1〜第3の発明において、前記段差エッジ特定手段が、暫定的に仮決定する前記段差エッジパラメータを、その初期値から逐次更新していくことによって、前記最適段差エッジパラメータを特定するように構成される場合には、該段差エッジ特定手段は、前記段差エッジパラメータの初期値により規定される暫定的な段差エッジが、前記上段側の平坦面部のうち、平面であることが事前に特定されている領域を通るように該初期値を設定する手段を含むことが好ましい(第4発明)。
この第4発明によれば、暫定的に仮決定する前記段差エッジパラメータの初期値を、上記の如く設定することによって、前記最適段差エッジパラメータを特定するための暫定的な段差エッジパラメータの更新処理の安定性(ロバスト性)を高め、暫定的な段差エッジパラメータが発散してしまうように更新されてしまうのを防止することができる。
この第4発明では、前記撮像手段が、前記段差部を移動する移動体に、該移動体の進行方向前方を撮像するように搭載されている場合には、前記段差エッジパラメータの初期値を設定する手段は、該初期値により規定される暫定的な段差エッジの方向が前記移動体の進行方向に対して交差する方向になるように該初期値を設定することが好ましい(第5発明)。
この第5発明よれば、暫定的に仮設定する段差エッジパラメータの初期値により規定される段差エッジの方向が、実際の方向に近い方向となるので、段差エッジパラメータの更新処理の安定性(ロバスト性)を効果的に高めることができる。
また、前記第1〜第5発明では、前記評価関数は、前記第1の撮像画像中に設定した第3の領域における画像の画素値分布と、前記第2の撮像画像中の前記第3の領域に対応する領域における画像を、前記上段側の平坦面部の一段下側の下段側の平坦面部を包含する下段側平面の位置及び姿勢を表すものとして事前に特定された下段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第3誤差関数を、前記第1誤差関数と第2誤差関数とにさらに線形結合して構成された評価関数であり、
前記評価関数算出用領域設定手段は、前記第3の領域が、前記第1の撮像画像中において前記第1の領域よりも前記下段側の平坦面部寄りの位置で該下段側の平坦面部の少なくとも一部分を含むと共に、該第3の領域の前記第1の領域側の境界線の該第1の撮像画像中での位置及び方向を、該第1の撮像画像中での前記暫定的な段差エッジの位置及び方向に応じて規定するように該第3の領域を設定する手段を含むようにしてもよい(第6発明)。
この第6発明によれば、前記評価関数は、前記第1誤差関数及び第2誤差関数に、さらに、前記第3誤差関数を加えた3つの誤差関数を線形結合することによって構成される。そして、この場合、前記第3誤差関数を算出するための第1の撮像画像中の第3の領域は、上記の如く設定される。
このため、第1及び第2の撮像画像における段差部の上段側の平坦面部及び段差エッジの近辺の撮像画像に加えて、下段側の平坦面部の近辺の撮像画像の画素値データが、暫定的に仮決定する段差エッジパラメータに依存性を有するように前記第3誤差関数を介して評価関数に反映されることとなる。この結果、この評価関数の値を最小化するように決定した最適段差エッジパラメータの信頼性と、該最適段差エッジパラメータの探索的な特定処理のロバスト性とを高めることができる。
この第6発明では、前記第3の領域を設定する手段は、該第3の領域の前記第1の領域側の境界線が、前記暫定的な段差エッジと平行に延在するように該第3の領域を設定することが好ましい(第7発明)。
この第7発明によれば、暫定的に仮決定する段差エッジパラメータを前記第3の領域に適切に反映させ、前記第3の誤差関数が、暫定的に仮決定する段差エッジパラメータに依存して変化するようにすることができる。ひいては、前記評価関数に、下段側の平坦面部の近辺の撮像画像の画素値データを適切に反映させることができる。
上記第6発明又は第7発明では、前記第3の領域を設定する手段は、前記第1の撮像画像において第1の領域と第3の領域との間に所定の間隔を存するように該第3の領域を設定するものであり、該所定の間隔は、前記暫定的な段差エッジが実際の段差エッジに一致する場合に、前記第3の領域の前記第1の領域側の境界線が、前記第1の撮像画像において前記下段側の平坦面部内に存在するように設定された間隔であることが好ましい(第8発明)。
これによれば、前記第1の領域と第3の領域との間に、上記所定の間隔を設定しておくことで、第1の領域と第3の領域との間の間隔内の画像は前記第3誤差関数、ひいては、前記評価関数の値に影響を及ぼさないものとなる。そして、この場合、上記所定の間隔は、前記暫定的な段差エッジが実際の段差エッジに一致する場合に、前記第3の領域の前記第1の領域側の境界線が、前記第1の撮像画像において前記下段側の平坦面部内に存在するように設定された間隔、すなわち、前記暫定的な段差エッジが実際の段差エッジに一致する場合に、第3領域内に前記上下段間部分の画像が含まれないように設定された間隔である。
このため、種々様々の形状の段差部に対して、上段側の平坦面部の先端部の前記起立面と下段側の平坦面部とのいずれとも異なる部分の画像が、前記第1誤差関数又は第3誤差関数の値、ひいては、前記評価関数の値に影響を及ぼすのを極力抑制することができる。その結果、一般的な形状の種々様々な段差部に対して、前記評価関数の信頼性(暫定的な段差エッジの位置及び方向の誤差を表すものとしての信頼性)を高め、ひいては、実際の段差エッジの位置及び方向の推定の信頼性を高めることができると共に、その推定処理の汎用性を高めることができる。
また、前記第6発明では、前記第3の領域を設定する手段は、次のような形態で前記第3の領域の前記第1の領域側の境界線を設定してもよい。
すなわち、前記第6発明において、前記第3の領域を設定する手段は、前記第3の領域の前記第1の領域側の境界線が、前記仮決定した段差エッジパラメータ及び前記上段側平面パラメータに応じて設定された前記起立平面パラメータに対応する前記起立平面と前記下段側平面との交線を前記第1の撮像画像に投影した直線上の線分になるように、該第3の領域の第1の領域側の境界線を設定するようにしてもよい(第9発明)。
この第9発明によれば、暫定的に仮決定する段差エッジパラメータを前記第3の領域に適切に反映させ、前記第3の誤差関数が、暫定的に仮決定する段差エッジパラメータに依存して変化するようにすることができる。ひいては、第6発明と同様に、前記評価関数に、下段側の平坦面部の近辺の撮像画像の画素値データを適切に反映させることができる。
さらに、種々様々の形状の段差部に対して、上段側の平坦面部の先端部の前記起立面と下段側の平坦面部とのいずれとも異なる部分の画像が、前記第1誤差関数又は第3誤差関数の値、ひいては、前記評価関数の値に影響を及ぼすのを極力抑制することができる。その結果、第7発明と同様に、一般的な形状の種々様々な段差部に対して、前記評価関数の信頼性(暫定的な段差エッジの位置及び方向の誤差を表すものとしての信頼性)を高め、ひいては、実際の段差エッジの位置及び方向の推定の信頼性を高めることができると共に、その推定処理の汎用性を高めることができる。
前記段差部をその下段側が手前側、上段側が奥側となる視線で前記撮像手段により撮像した場合には、前記第1〜第9発明によって段差エッジの位置及び方向を推定することができる。一方、前記段差部をその上段側が手前側、下段側が奥側となる視線で前記撮像手段により撮像した場合には、階段の段鼻の先端面の如き段差部の起立面は、前記第1の撮像画像及び第2の撮像画像には写らない。この場合には、以下に説明する第10発明によって、段差エッジの位置及び方向を推定することができる。
すなわち、第10発明の段差エッジ推定装置は、第1の撮像画像及び第2の撮像画像から構成されるステレオ画像を撮像するための撮像手段を用い、下段側の平坦面部と、該下段側の平坦面部の一段上側の上段側の平坦面部との組を少なくとも一組有する段差部を、その上段側が手前側、下段側を奥側となる視線で前記撮像手段により撮像し、その撮像により得られた該段差部のステレオ画像を基に、前記上段側の平坦面部の先端側エッジである段差エッジの3次元空間中の位置及び方向を推定する段差エッジ推定装置であって、
前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値を、暫定的に仮決定した段差エッジパラメータと、前記上段側の平坦面部を包含する上段側平面の位置及び姿勢を表すものとして事前に特定された上段側平面パラメータと、前記上段側の平坦面部の一段下側の下段側平坦面部を包含する下段側平面の位置及び姿勢を表すものとして事前に特定された下段側平面パラメータとを用いて算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する段差エッジ特定手段を備え、
前記評価関数は、
前記第1の撮像画像中に設定した第4の領域における画像の画素値分布と、前記第2の撮像画像中の前記第4の領域に対応する領域における画像を前記上段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第4誤差関数と、
前記第1の撮像画像中に設定した第5の領域における画像の画素値分布と、前記第2の撮像画像中の前記第5の領域に対応する領域における画像を前記下段側平面パラメータに応じて射影変換した場合に得られる画像の画素値分布との誤差を表す第5誤差関数とを線形結合して構成された評価関数であり、
前記段差エッジ特定手段は、前記第1の撮像画像中において、前記第4の領域が、前記上段側の平坦面部と前記下段側の平坦面部とのうちの上段側の平坦面部寄りの領域、前記第5の領域が前記上段側の平坦面部と前記下段側の平坦面部とのうちの下段側の平坦面部寄りの領域となると共に、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第1の撮像画像中において前記第4の領域の前記第5の領域側の境界線となり、且つ、前記第5の領域の前記第4の領域側の境界線が、前記第4の領域の前記第5の領域側の境界線に一致するか、又は、前記第4の領域の前記第5の領域側の境界線と間隔を存して当該第5の領域側の境界線と平行に延在するように該第4の領域及び第5の領域を設定する評価関数算出用領域設定手段を含むことを特徴とする(第9発明)。
ここで、前記段差部をその上段側が手前側、下段側が奥側となる視線で前記撮像手段により撮像した場合には、前記第1発明に関して説明した上段側平坦面部の先端部の起立面が第1の撮像画像及び第2の撮像画像に投影されず、それらの第1の撮像画像及び第2の撮像画像中では、段差エッジは、上段側の平坦面部と下段側の平坦面部との境界線として写ることとなる。上記第10発明は、このことを考慮して構築されたものである。
この第10発明によれば、前記段差エッジ特定手段は、前記段差エッジの位置及び方向を表す段差エッジパラメータに依存して値が変化する関数としてあらかじめ定められた所定の評価関数の値の大きさを最小化するように、前記暫定的に仮決定した段差エッジパラメータを修正することによって、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定する。
この場合、第10発明における上記評価関数は、前記第4誤差関数と第5誤差関数とを線形結合して構成される。そして、この第4誤差関数は、前記第1の撮像画像中に設定した第4の領域における画像の画素値分布と、前記第2の撮像画像中の前記第4の領域に対応する領域における画像を、前記上段側平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す関数である。
また、第5誤差関数は、前記第1の撮像画像中に設定した第5の領域における画像の画素値分布と、前記第2の撮像画像中の前記第5の領域に対応する領域における画像を、前記下段側平面パラメータに応じて射影変換した場合に得られる射影変換画像の画素値分布との誤差を表す関数である。
さらに、前記第4の領域と第5の領域とは、前記した如く、前記第1の撮像画像中において、前記第4の領域が、前記上段側の平坦面部と前記下段側の平坦面部とのうちの上段側の平坦面部寄りの領域、前記第5の領域が前記上段側の平坦面部と前記下段側の平坦面部とのうちの下段側の平坦面部寄りの領域となると共に、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第1の撮像画像中において前記第4の領域の前記第5の領域側の境界線となり、且つ、前記第5の領域の前記第4の領域側の境界線が、前記第4の領域の前記第5の領域側の境界線に一致するか、又は、前記第4の領域の前記第5の領域側の境界線と間隔を存して当該第5の領域側の境界線と平行に延在するように設定される領域である。
このため、第10発明における前記評価関数の値は、第1発明における評価関数と同様に、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジの位置及び方向の、実際の段差エッジの位置及び方向に対する誤差の度合いを表すものとなり、暫定的に仮決定した段差エッジパラメータにより規定される段差エッジの位置及び方向が、実際の段差エッジの位置及び方向に合致していれば、前記評価関数の値は、最小となるものである。
従って、暫定的に仮決定した段差エッジパラメータに対して前記評価関数を算出し、その算出結果を基に、前記評価関数の値の大きさを最小化するように、前記暫定的に仮決定した段差エッジパラメータを修正することによって、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータを特定できることとなる。
よって、第10発明によれば、前記段差エッジが、第1及び第2の撮像画像中において、上段側の平坦面部と下段側の平坦面部との境界線として写ることを前提とすることによって、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジの位置及び方向の、実際の段差エッジの位置及び方向に対する誤差の度合いを、前記した如く構成した評価関数の値によって適正に評価することができる。このため、撮像手段により取得される第1の撮像画像及び第2の撮像画像から構成されるステレオ画像を利用して、階段等の段差部の平坦面部の先端部のエッジの位置及び方向を精度良く推定することが可能となる。
なお、第4誤差関数の代表的な例としては、前記第1の撮像画像中の第4の領域における画像の各画素値(輝度値など)と、前記第2の撮像画像中の前記第4の領域に対応する領域における画像の射影変換画像の各画素値との偏差の絶対値又は二乗値の積算値、あるいは、該絶対値又は二乗値の平均値が挙げられる。第5誤差関数についても同様である。
また、第10発明において、前記最適段差エッジパラメータを特定するための代表的な手法としては、前記第1〜第3発明に関して説明した第1の手法又は第2の手法と同じ手法を用いることができる。
上記第10発明において、所謂、オクルージョンによって、第1の撮像画像に写っている下段側の平坦面部のうちの一部、例えば、下段側の平坦面部のうちの、上段側の平坦面部の段差エッジに近い部分が第2の撮像画像には写っていないという場合がある。
そして、このようなオクルージョンが発生している場合には、前記第1の撮像画像において前記第5の領域の前記第4の領域側の境界線が、前記第4の領域の前記第5の領域側の境界線に一致するようにした場合(前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジが、前記第1の撮像画像中において前記第4の領域と第5の領域との間の境界線となる場合)には、暫定的な段差エッジが実際の段差エッジに一致していても、前記第5誤差関数に上記オクルージョンに起因する誤差成分が含まれることとなる。
従って、上記の如きオクルージョンが発生する状況では、前記第5の領域の前記第4の領域側の境界線が、前記第4の領域の前記第5の領域側の境界線と間隔を存して当該第5の領域側の境界線と平行に延在するように第5の領域を設定して、第5の領域に、第2の撮像画像中に写らないような部分が含まれないようにすることが好ましい。
そこで、第10発明では、前記評価関数算出用領域設定手段は、前記仮決定した段差エッジパラメータにより規定される暫定的な段差エッジを前記第1の撮像画像に投影してなる線である第1暫定段差エッジ線を、前記上段側平面パラメータに応じて第1の撮像画像側から第2の撮像画像側に射影変換した場合に得られる第2A暫定段差エッジ線と、前記第1暫定段差エッジ線を、前記下段側平面パラメータに応じて第1の撮像画像側から第2の撮像画像側に射影変換した場合に得られる第2B暫定段差エッジ線との第2の撮像画像中での位置関係を比較し、該2の撮像画像での第2A暫定段差エッジ線の位置が、前記第2B暫定段差エッジ線よりも、前記上段側の平坦面部から下段側の平坦面部に向う向きに変位した位置であるか否かを判断する手段と、該判断結果が肯定的である場合には、前記第5の領域の前記第4の領域側の境界線が、前記第2A暫定段差エッジ線を、前記下段側平面パラメータに応じて第2の撮像画像側から第1の撮像画像側に射影変換した場合に得られる線上に存在するように前記第5の領域を設定し、前記判断結果が否定的である場合には、前記第5の領域の前記第4の領域側の境界線が、前記第1暫定段差エッジ線上に存在するように前記第5の領域を設定する手段とを備えることが好ましい(第11発明)。
この第11発明によれば、第1の撮像画像に写っている下段側の平坦面部のうち、上段側の平坦面部の段差エッジに近い部分が第2の撮像画像には写っていないというようなオクルージョンが発生していても、前記暫定的な段差エッジが、実際の段差エッジに一致もしくはほぼ一致するような状況で、上記オクルージョンに起因する誤差成分が前記第5誤差関数に極力含まれないようにすることができる。
ひいては、前記暫定的な段差エッジと実際の段差エッジとの一致度合いを反映するものとしての前記誤差関数の値の信頼性を高めることができる。その結果、実際の段差エッジの位置及び方向の推定値を表すものとしての最適段差エッジパラメータの信頼性をより一層高めることができる。
本発明の一実施形態を図1〜図11を参照して以下に説明する。
図1(a),(b)を参照して、本実施形態では、移動体としての脚式移動ロボット1によって階段等の段差部の段差エッジの位置及び方向を推定する場合を例にとって説明する。図示例の脚式移動ロボット1(以下、単にロボット1という)は、左右一対の2つの脚体を備える2足歩行ロボットである。
このロボット1には、2つの撮像画像から構成されるステレオ画像を撮像する撮像手段として、左右一対のカメラ3R,3Lが搭載され、これらのカメラ3R,3Lによって、ロボット1の周辺環境(主に、ロボット1の進行方向前方の環境)のステレオ画像を取得することが可能となっている。なお、カメラ3R,3Lの撮像画像は、モノトーン画像及びカラー画像のいずれの画像であってもよい。
そして、ロボット1は、これらのカメラ3R,3Lによって取得したステレオ画像等を基に、周辺環境の床形状や設置物等を認識しつつ移動する。
例えば、図1(a)に示すように、ロボット1がその進行方向前方に存在する段差部としての階段50を昇ろうとしている状況、あるいは、図1(b)に示すように、ロボット1がその進行方向前方に存在する段差部としての階段50を降ろうとしている状況において、ロボット1は、カメラ3R,3Lによって取得した進行方向前方の環境のステレオ画像を基に、階段50の各段の平坦面部51(詳しくは、ほぼ水平な平坦面として認識し得る部分)の3次元空間中の位置及び姿勢(法線方向)や、各段の平坦面部51の先端側エッジとしての段差エッジ52の3次元空間中の位置及び方向を認識する。
ここで、ロボット1が認識する階段50の各段の平坦面部51の段差エッジ52は、図1(a)に示すロボット1の如く、階段50をその下段側が手前側、上段側が奥側となる視線で見た場合においては、上段側の平坦面部51の外形の境界線のうち、手前側で横方向(階段50の幅方向)に直線状に延在する境界線である。
また、図1(b)に示すロボット1の如く、階段50をその上段側が手前側、下段側が奥側となる視線で見た場合にいては、段差エッジ52は、上段側の平坦面部51の外形の境界線のうち、奥側で横方向(階段50の幅方向)に直線状に延在する境界線である。
補足すると、図2を参照して、階段の各段の平坦面部は、一般に踏面と言われる部分である。そして、この踏面の先端部は、一般に段鼻と言われる部分である。従って、上記階段50の各段の平坦面部51は、換言すれば、階段50の各段の踏面を意味し、また、上記段差エッジ52は、換言すれば、階段50の各段の段鼻の上面の先端側境界線を意味する。
なお、階段の上段側の踏面(平坦面部)の先端側と、その一段下側の下段側の踏面(平坦面部)の奥側との間での部分は、一般に蹴込みと言われる。そして、階段の各段の踏面(平坦面部)の先端部(段鼻)は、多くの場合、図2に示す如く、その先端面が、上記蹴込みの箇所に起立した姿勢で形成される面部(以降、蹴込み面部という)よりも若干突き出るように形成される。
但し、本実施形態で、段差エッジの推定対象として主に説明する階段では、段鼻の先端面が、蹴込み面部よりも若干突き出ている必要はない。例えば、図1(a),(b)に簡略的に記載した階段50のように、段鼻の先端面と蹴込み面部とが面一もしくはほぼ面一に形成されていてもよい。また、本実施形態で、段差エッジの推定対象として主に説明する階段は、蹴込みの箇所に、蹴込み面部を形成する板部材が設けられていない(蹴込み面部が存在しない)構造のものであってもよい。
図3を参照して、ロボット1には、CPU、RAM、ROMを含む電子回路ユニットにより構成された演算処理装置10が搭載されており、カメラ3R,3Lによりそれぞれ撮像された撮像画像の画素値データ(撮像画像を構成する画素毎の輝度、彩度、色相等を表すデータ)が演算処理装置10に入力されて、図示しない画像メモリに逐次更新されつつ記憶保持されるようになっている。
演算処理装置10は、実装されたプログラムによって実現される機能として、ロボット1の進行方向前方に存在する平坦面部の位置及び姿勢(法線方向)を認識する平坦面部認識手段11と、階段等の段差部の段差エッジの位置及び方向を表す段差エッジパラメータを探索的な手法によって特定する段差エッジ特定手段12とを備えており、これらの手段11,12の処理によって、本発明の段差エッジ推定装置としての機能を実現する。なお、演算処理装置10は、段差エッジ推定装置としての機能の他に、ロボット1の動作制御を行なう機能を備えていてもよい。
上記平坦面部認識手段11及び段差エッジ特定手段12のより詳細な処理を以下に説明する。
カメラ3R,3Lによって撮像されたロボット1の進行方向前方の環境のステレオ画像を構成する2つの撮像画像の画素値データが平坦面部認識手段11に与えられる。そして、平坦面部認識手段11は、与えられた2つの撮像画像の画素値データから、公知のステレオマッチングの手法によって、両撮像画像に捉えられている物体の各部の空間的な位置を算出する。その空間的な位置は、カメラ3R,3Lに対して固定されたカメラ座標系での位置である。該カメラ座標系としては、本実施形態では、図4(a),(b)に例示する如く、カメラ3R,3Lのいずれか一方の光学中心、例えばカメラ3Rの光学中心C0に原点を設定した3軸直交座標系(xyz座標系)Ccamが用いられる。
さらに、平坦面部認識手段11は、算出した位置データを基に、各撮像画像(あるいはいずれか一方の撮像画像)において、空間的な高さがほぼ一定となる領域(該領域内の各部に対応する空間的な高さが互いにほぼ同一となる領域)、すなわち、ほぼ水平な面と見なせる領域が投影された画像部分を平坦面部投影領域として抽出する。
例えば、図1(a)又は(b)に示した状況で、階段50をカメラ3R,3Lにより撮像し、カメラ3R,3Lのいずれか一方、例えばロボット1の右側のカメラ3Rによって、図5(a),(b)あるいは図6(a),(b)に示す如き撮像画像が得られた場合を想定する。図5(a),(b)は、ロボット1の進行方向前方に昇り階段が存在する状況でのカメラ3Rの撮像画像を例示しており、図6(a),(b)は、ロボット1の進行方向前方に降り階段が存在する状況でのカメラ3Rの撮像画像を例示している。
この場合、カメラ3Rの撮像画像において、図5(a),(b)で参照符号Ar1,Ar2を付した領域、あるいは、図6(a),(b)で参照符号Ar3,Ar4,Ar5,Ar6を付した領域が平坦面部投影領域として抽出される。
補足すると、2つの撮像画像(ステレオ画像)から空間的な位置データが算出された各部の高さは、例えば、カメラ座標系Ccamでの位置データを、ロボット1の支持脚側の接地面(該脚体の足平部と床面との接触面)に設定される座標系に座標変換することで、認識することができる。この場合、座標変換は、ロボット1の各関節の変位量の検出値又は目標値に応じて行なえばよい。
次いで、平坦面部認識手段11は、抽出した平坦面部投影領域に対応する平坦面部を包含する平面の空間的な位置及び姿勢(法線方向)を表す平面パラメータを算出する。この平面パラメータは、本実施形態では、平面の法線方向の単位ベクトル↑nを、カメラ3R,3Lのうちの一方、例えばカメラ3Rの光学中心C0と平面との間の距離d(光学中心C0から平面への垂線の長さ)により除算してなるベクトル↑m(=↑n/d)である。以降、この↑mを平面パラメータベクトル↑mという。なお、本明細書では、“↑”をベクトルを表現する記号として使用している。
上記平面パラメータベクトル↑mは、公知の手法によって算出される。例えば、平坦面部投影領域の各部に対応する空間的な位置データを用いて、↑mを算出することができる。あるいは、例えば、カメラ3R,3Lにより撮像した2つの撮像画像(ステレオ画像)から、本願出願人が特開2008−250516号公報にて先に提案した手法のように、射影変換(平面射影変換)を利用した探索的な演算処理によって↑mを算出することもできる。
このようにして平坦面部認識手段11によって算出される平面パラメータベクトル↑mの例を図4(a),(b)に示す。図4(a),(b)は、例えば図1(a)に示した状況で、階段50(昇り階段)をカメラ3R,3Lにより撮像した場合の例を示しており、図中の↑ma、↑mbが平坦面部認識手段11の処理によって算出される平面パラメータベクトルである。↑maは、階段50の上段側の平坦面部51aを包含する上段側平面PLaに対応する平面パラメータベクトル、↑mbは、上段側の平坦面部51aの一段下側の下段側の平坦面部51bを包含する下段側平面PLbに対応する平面パラメータである。
なお、図4(b)中のda,dbは、それぞれカメラ3Rの光学中心C0と、上段側平面PLa、下段側平面PLbとの距離を示しており、上段側平面PLa、下段側平面PLbのそれぞれの法線方向の単位ベクトルをそれぞれ↑na、↑nbとおくと、↑ma=↑na/da、↑mb=↑nb/dbとなる。
図1(b)に示した状況で、階段50(降り階段)をカメラ3R,3Lにより撮像した場合においても、上記と同様に、図3(a),(b)に示した平面パラメータベクトル↑ma,↑mbが平坦面部認識手段11の処理によって算出される。
補足すると、平面パラメータは、平面の空間的な位置および姿勢(法線方向)を表すものであればどのようなものでもよい。例えば、平面パラメータベクトル↑mの代わりに、平面の法線方向の単位ベクトル↑nと、光学中心C0から平面までの距離dとの組(↑n,d)を平面パラメータとして用いてもよい。あるいは、例えば平面上の3点の空間的な位置の組を平面パラメータとして用いることもできる。
以上が平坦面部認識手段11の処理の詳細である。
次に、段差エッジ特定手段12に関して説明する。段差エッジ特定手段12は、ロボット1の進行方向前方に階段等の段差部が存在する場合に、該段差部の各段の平坦面部(ロボット1の遊脚側の脚体の足平部を着地させようとする平坦面部)の実際の段差エッジの位置及び方向を表す段差エッジパラメータを探索的に特定する演算処理を実行することで、段差部の段差エッジの空間的な(3次元空間中の)位置及び方向を推定する。この場合、段差エッジパラメータを探索的に特定する演算処理では、段差エッジパラメータを暫定的に仮決定し、その暫定的に仮決定した段差エッジパラメータを後述する評価関数E1の値に基づいて適宜修正することで、実際の段差エッジの位置及び方向の推定値を表す段差エッジパラメータとしての最適段差エッジパラメータを特定する。
上記段差エッジパラメータは、それによって、段差エッジの空間的な位置及び方向を規定できるものであればどのようなものでもよい。本実施形態では、例えば、段差エッジを形成する直線(段差エッジに重なる直線)の空間的な方向を示すベクトル(段差エッジと同方向のベクトル)と、該直線上の1つの点の空間的な位置との組、あるいは、段差エッジを形成する直線上の互いに異なる2つの点の空間的な位置の組により構成される。
なお、例えば、段差エッジを形成する空間的な直線を、カメラ3R,3Lのいずれか一方の撮像画像に投影することによって該撮像画像中に得られる二次元直線の、該撮像画像上での二次元的な位置及び方向を表すパラメータ(例えば、該二次元直線の二次元的な方向を示すベクトルと、該二次元直線上の1つの点の位置との組、もしくは、該二次元直線上の互いに異なる2つの点の空間的な位置の組)を、段差エッジパラメータとして採用してもよい。段差エッジは、上段側の平坦面部と同じ平面上に存在するので、該段差エッジの撮像画像中での二次元的な位置及び方向が決まれば、その位置及び方向と、上段側の平坦面部を包含する平面(上段側平面PLa)の空間的な位置及び姿勢を表す平面パラメータ(例えば前記平面パラメータベクトル)とから、段差エッジの空間的な位置及び姿勢を特定することが可能である。
ここで、ロボット1の進行方向前方に階段等の段差部が存在する場合に、段差エッジ特定手段12が、実際の段差エッジに対応する段差エッジパラメータ(最適段差エッジパラメータ)を探索的に特定するために実行する演算処理は、カメラ3R,3Lの視線方向(カメラ3R,3Lから段差部を見る向き)が、段差部の下段側の平坦面部が手前側、上段側の平坦面部が奥側となる方向である場合(例えば図1(a)に示す場合。以降、カメラ視線が下・上方向の場合ということがある)と、カメラ3R,3Lの視線方向が、段差部の上段側の平坦面部が手前側、下段側の平坦面部が奥側となる方向である場合(例えば図1(b)に示す場合。以降、カメラ視線が上・下方向の場合ということがある)とで一部の処理が異なる。
これは、カメラ視線が下・上方向の場合には、上段側の平坦面部とその一段下側の平坦面部との間の蹴込みの箇所の画像が各撮像画像内に含まれることとなる一方、カメラ視線が上・下方向の場合には、蹴込みの箇所の画像が各撮像画像内に含まれないこととなるからである。
まず、カメラ視線が下・上方向の場合の段差エッジ特定手段12の処理を説明する。この説明においては、段差部が階段50であって、カメラ3Rの撮像画像が、図5(a),(b)に示したような画像となる場合を主要例として段差エッジ特定手段12の処理を説明する。この場合、図5(a),(b)に示す平坦面部投影領域Ar1,Ar2にそれぞれ対応する階段50の平坦面部は、それぞれ、図4(a),(b)に示す上段側の平坦面部51a、その一段下側の下段側の平坦面部51bであるとする。なお、以降の説明では、カメラ3R,3Lの撮像画像のうちの一方、例えば、カメラ3Rの撮像画像を第1の撮像画像、カメラ3Lの撮像画像を第2の撮像画像と称する。また、以降の説明では、暫定的に仮決定する段差エッジパラメータを暫定段差エッジパラメータ、該暫定段差エッジパラメータにより位置及び方向が規定される直線(暫定的な段差エッジを形成する直線)を暫定段差エッジラインLeと称する。
カメラ視線が下・上方向の場合、段差エッジ特定手段12は、図7のブロック図に示す処理を実行するとで、実際の段差エッジに対応する段差エッジパラメータ(最適段差エッジパラメータ)を探索的に特定する。
段差エッジ特定手段12は、まず、S1の処理を実行し、暫定段差エッジパラメータの初期値を設定する。この場合、暫定段差エッジパラメータの初期値により規定される暫定段差エッジラインLeが、ロボット1の進行方向に対して交差する方向(進行方向を横切る方向)に延在すると共に、該暫定段差エッジラインLeが、カメラ3R,3Lによって撮像された階段50の上段側の平坦面部51a(推定しようとする段差エッジを先端に有する平坦面部51a)のうちの、平坦面部認識手段11によって抽出された平坦面部投影領域に対応する領域を通るように、暫定段差エッジパラメータの初期値が設定される。
例えば、図4(a)に二点鎖線で示す直線Le(0)が、暫定段差エッジパラメータの初期値により規定される暫定段差エッジラインLeとされる。この場合、第1の撮像画像上においては、上記暫定段差エッジラインLe(=Le(0))を第1の撮像画像に投影してなる線(二次元直線)は、図5(a)に示すラインLep(0)となる。なお、以降、暫定段差エッジラインLeを第1の撮像画像に投影してなる二次元直線を、画像上暫定段差エッジラインLepという。
上記暫定段差エッジパラメータの初期値のより具体的な設定手法としては、例えば次のような手法が挙げられる。その1つの手法では、例えば、階段50の上段側の平坦面部51aに対応して平坦面部認識手段11によって抽出された平坦面部投影領域Ar1の内部に1つの点F1を設定し、その点F1に対応する空間上の位置の点(上段側の平坦面部51aの点)から下段側の平坦面部51bを包含する平面PLbに降ろした垂線を第1の撮像画像に投影してなる線F2を求める。さらに、第1の撮像画像中で、当該投影線F2に直交し、且つ、上記点F1を通る直線F3を求める。そして、暫定段差エッジパラメータの初期値により規定される画像上暫定エッジラインLep(0)が、上記直線F3に合致するように、暫定段差エッジパラメータの初期値を設定する。
あるいは、他の1つの手法では、第1の撮像画像中で、階段50の上段側の平坦面部51aに対応する平坦面部投影領域Ar1内で、ロボット1の進行方向に延在し、横方向に間隔を有する2つの所定長の線分G1,G2を設定し、それらの線分G1,G2の中点を結ぶ直線G3を求める。そして、暫定段差エッジパラメータの初期値により規定される画像上暫定エッジラインLep(0)が、上記直線G3に合致するように、暫定段差エッジパラメータの初期値を設定する。
なお、暫定段差エッジパラメータの初期値の設定の仕方は、上記の例に限らず、種々様々な手法を採用できる。
補足すると、本実施形態では、段差エッジ特定手段12は、第1の撮像画像に、図5(a),(b)に破線で示す如き方形状の枠内領域AR1を画像処理領域として設定し、この画像処理領域AR1での撮像画像を、実際の段差エッジに対応する段差エッジパラメータを探索的に特定するための演算処理に利用する。この場合、第1の撮像画像において、画像処理領域AR1の上部が、階段50の上段側の平坦面部51aに対応する平坦面部投影領域Ar1に存在し、且つ、画像処理領域AR1の下部が、階段50の下段側の平坦面部51bの平坦面部投影領域Ar2に存在するように、該画像処理領域AR1が第1の撮像画像中に設定される。なお、画像処理領域AR1の横方向の幅は、例えば、概ねロボット1の通過領域と同程度の幅となるように設定される。
そして、暫定段差エッジパラメータの初期値は、それにより規定される画像上暫定段差エッジラインLep(0)の画像処理領域AR1内の部分が、図5(a)に示す如く、階段50の上段側の平坦面部51aに対応する平坦面部投影領域Ar1内に含まれるように設定することが望ましい。
なお、S1の処理では、後に、暫定段差エッジパラメータの更新処理も行なうが、これについては後述する。
段差エッジ特定手段12は、次に、図7のS2の処理と、S3の処理とを実行する。このうち、S2の処理では、段差エッジ特定手段12は、S1で設定した暫定段差エッジパラメータにより規定される暫定段差エッジラインLeを含む(該暫定段差エッジラインLeで上段側の平坦面部51aを包含する上段側平面PLaと交差する)と共に、上段側の平坦面部51aを包含する上段側平面PLaに対して起立した姿勢となる起立平面PLc(図4(a),(b)を参照)の位置及び姿勢を表す平面パラメータを設定する。
この場合、起立平面PLcの平面パラメータは、上段側平面PLa及び下段側平面PLbの平面パラメータとしての平面パラメータベクトル↑ma,↑mbと同様に、起立平面PLcの法線方向の単位ベクトル↑ncを、カメラ3Rの光学中心C0と該起立平面PLcとの距離dc(図4(b)を参照)により除算してなる平面パラメータベクトル↑mc(=↑nc/dc)である。
ここで、起立平面PLcは、階段の段鼻の先端面を包含する平面に相当するものである。そして、階段の各段の段鼻の先端面は、一般に、その段の平坦面部(踏面)に対してほぼ垂直に起立した面となっている。
そこで、本実施形態では、段差エッジ特定手段12は、S2において、起立平面PLcが上段側平面PLaに対して垂直に起立し、且つ、S1で設定した暫定段差エッジパラメータにより規定される暫定段差エッジラインLeを含むように、起立平面パラメータベクトル↑mcを設定する。
例えば、暫定段差エッジラインLeが図4(a)に示したLe(0)である場合、図4(a),(b)に示すように、起立平面パラメータベクトル↑mcが設定される。
この場合、上記起立平面PLcの空間的な位置及び姿勢(カメラ座標系Ccamでの位置及び姿勢)は、カメラ座標系Ccamでの上段側平面PLaの姿勢と、暫定段差エッジラインLeの位置及び姿勢により一義的に定まることとなる。そこで、S2の処理では、段差エッジ特定手段12は、S1で設定した暫定段差エッジパラメータと、平坦面部認識手段11で先に算出した上段側平面パラメータベクトル↑maとに応じて、起立平面パラメータベクトル↑mcを設定する。
なお、上段側平面PLaに対する起立平面PLcの傾斜角度は、必要に応じて適宜、90°以外の角度に設定してもよい。例えば、段差エッジを推定しようとする階段の各段鼻の先端面が、踏面に対してほぼ同じ傾斜角度で傾斜した姿勢となっており、その傾斜角度(平均的な傾斜角度)が事前の計測等により判明しているような場合には、その傾斜角度だけ、起立平面PLcが上段側平坦面PLaに対して傾斜させた姿勢となるように、起立平面パラメータを設定するようにしてもよい。
前記S3の処理では、段差エッジ特定手段12は、第1の撮像画像中に、後述の射影変換(平面射影変換)を行なうために用いる3つの領域Aa1,Ab1,Ac1を、S1で設定した暫定段差エッジパラメータに応じて設定する。
第1の撮像画像中の3つの領域Aa1,Ab1,Ac1は、第1の撮像画像中に前記した如く設定した画像処理領域AR1内で図5(a),(b)に例示する如く設定される。これらの3つの領域Aa1,Ab1,Ac1は、それぞれ、本発明における第2の領域、第3の領域、第1の領域に相当するものであり、これらの設定の仕方の基本的な指針は次の通りである。
すなわち、領域Aa1,Ac1については、基本的には、第1の撮像画像において、画像上暫定段差エッジラインLepが領域Aa1,Ac1の境界線となると共に、領域Ac1が、上段側の平坦面部51aと下段側の平坦面部51bとの間の蹴込みの箇所(これは、本発明における上下段間部分に相当する)と、上段側の平坦面部51aとのうちの蹴込みの箇所寄りの領域、領域Aa1が、上段側の平坦面部51a寄りの領域となるように、これらの領域Aa1,Ac1の画像処理領域AR1内で設定される。
従って、図5(a),(b)に示す第1の撮像画像においては、領域Aa1,Ac1は、画像処理領域AR1内で、画像上暫定段差エッジラインLep(図5(a)ではLep(0)、図5(b)ではLep(n))を間に挟んで上下に隣接するように設定される。
この場合、領域Aa1の上側の境界線は、画像処理領域AR1の上端の境界線に一致するものとされる。すなわち、画像処理領域AR1の画像上暫定段差エッジラインLepの上側の全体の領域が、領域Aa1として設定される。
一方、領域Ac1の下側の境界線(画像上暫定段差エッジラインLep(Lep(0),Lep(n))と反対側の境界線)については、図5(a),(b)に例示する如く、画像上暫定段差エッジラインLepと所定の間隔Δcを存して平行に延在するように設定される。その間隔Δcは、本実施形態では、段鼻が踏面の先端部に形成された一般的な階段をカメラ3R,3Lにより撮像した場合に、それぞれの撮像画像における段鼻の厚みとほぼ同程度になるようにあらかじめ設定されている。
なお、撮像画像における段鼻の厚みは、カメラ3R,3Lから段鼻への距離によって変化するので、例えば、暫定段差エッジラインの近辺での上段側の平坦面部51aの距離(カメラ3R又は3Lからの距離)に応じて、領域Ac1の上記間隔Δcを可変的に設定するようにしてもよい。
また、領域Ab1については、基本的には、第1の撮像画像において、領域Ac1よりも下段側の平坦面部51b寄りの位置で該下段側の平坦面部51bの少なくとも一部分を含むと共に、領域Ab1の領域Ac1側の境界線の第1の撮像画像中での位置及び方向が、画像上暫定段差エッジラインLepの位置及び方向に応じて規定されるように、該領域Ab1が画像処理領域AR1内で設定される。
この場合、領域Ab1の上側の境界線は、領域Ac1の下側の境界線と所定の間隔Δbを存して平行に延在するように設定される。その間隔Δbは、本実施形態では、画像上暫定段差エッジラインLepが、第1の撮像画像における実際の段差エッジ52の線に一致もしくはほぼ一致する場合(図5(b)に示す場合)に、階段50の上段側の平坦面部51aと下段側の平坦面部51bとの間の蹴込みの箇所が、領域Ab1から除外されると共に、領域Ab1の領域Ac1側の境界線が下段側の平坦面部51bの画像内に存在するようにあらかじめ設定されている。
なお、領域Ac1に係わる上記間隔Δcと同様に、領域Ab1に係わる上記間隔Δbを、例えば、下段側の平坦面部51bの奥側部分の距離(カメラ3R又は3Lからの距離)に応じて可変的に設定するようにしてもよい。
そして、領域Ab1の下側の境界線は、画像処理領域AR1の下端の境界線に一致するものとされる。すなわち、画像処理領域AR1のうち、上記の如く設定される領域Ab1の上側の境界線の下側の全体の領域が、領域Ab1として設定される。
なお、本実施形態では、上記S3の処理によって、本発明における評価関数算出用領域設定手段を実現される。
以上の如く、S2、S3の処理を実行した後、段差エッジ特定手段12は、次に、S4、S5、S6の処理を実行する。このS4、S5、S6では、段差エッジ特定手段12は、それぞれ、S3で設定した第1の撮像画像の領域Aa1、領域Ab1、領域Ac1にそれぞれ対応する第2の撮像画像中の領域の画像に射影変換(平面射影変換)を施した画像を算出する。以降の説明では第1の撮像画像の領域Aa1、領域Ab1、領域Ac1にそれぞれ対応する第2の撮像画像中の領域を、それぞれ、領域Aa2、領域Ab2、領域Ac2と表記する。
ここで、上記射影変換は、一般的に言えば、ステレオ画像を構成する2つの撮像画像に、階段の踏面の如き平坦面部(平面部分)を撮像した場合に、一方の撮像画像中における当該平坦面部の画像を、他方の撮像画像における当該平坦面部の画像に変換するものである。その変換を行なう射影変換行列Pは、カメラ3R,3Lの2つの撮像画像のそれぞれの画像座標(各撮像画像における各画素の位置を表す二次元座標)を同次座標で表現した場合に、公知のように次式(1−1)により表される行列である。
P=R+↑t・↑mT ……(1−1)
式(1−1)におけるRは、カメラ3R,3Lのそれぞれのカメラ座標系の間の回転行列、↑tはそれらのカメラ座標系の間の並進移動ベクトル(ここでは縦ベクトル)、↑mは前記した如く定義される平面パラメータベクトル(ここでは縦ベクトル)であり、↑mTは、↑mの転置を意味する。
この場合、回転行列R及び並進移動ベクトル↑tは、カメラ3R,3Lの相互の相対的な位置関係及び姿勢関係に依存して定まるものであり、本実施形態ではあらかじめ特定されている。
補足すると、第1の撮像画像から第2の撮像画像への射影変換行列と、これの逆変換の射影変換行列(第2の撮像画像から第1の撮像画像への射影変換行列)とは、いずれも、式(1−1)の右辺の形で表現されるものの、上記R,↑t,↑mの値は、それぞれの射影変換行列で異なる。
具体的には、式(1−1)により表現される射影変換行列Pを第1の撮像画像及び第2の撮像画像のうちの一方から他方へのへの射影変換行列とした場合、その逆変換の射影変換行列(以降、これをP-1と表記する)は、次式(1−2)の但し書きの式(1−2a)〜(1−2d)により定義されるR’、↑t’、↑m’、↑d’を用いて、上記式(1−1)と同様の形の式(1−2)により表される。
P-1=R'+↑t’・(↑m’)T ……(1−2)
但し、
R’≡RT ……(1−2a)
↑t’≡−RT・↑t ……(1−2b)
↑m’≡R・↑m/d’ ……(1−2c)
↑d’≡d・(1+↑mT・RT・↑t) ……(1−2d)
以降の本実施形態の説明では、便宜上、第1の撮像画像から第2の撮像画像への射影変換行列を式(1−1)により表される射影変換行列Pとし、第2の撮像画像から第1の撮像画像への射影変換行列を式(1−2)により表される射影変換行列P-1とする。
以上を前提としてS4,S5,S6の処理を、図8を参照しつつ以下に具体的に説明する。なお、図8は、S4、S5,S6の処理を包括的に説明するための図であり、図中の符号中の「x」は、S4の処理では「a」、s5の処理では「b」、S6の処理では「c」を意味するものであるとする。
S4では、段差エッジ特定手段12は、平坦面部認識手段11により先に算出された上段側平面PLaの平面パラメータベクトル↑maを式(1−1)の↑mに代入することで射影変換行列Paを決定し、このPaを用いて、第1の撮像画像の領域Aa1に対応する第2の撮像画像中の領域Aa2を求める。具体的には、図8に示す如く、第1の撮像画像の領域Aa1の外形枠にPaによる射影変換を施すことによって、第2の撮像画像中の領域Aa2を求める。
さらに、段差エッジ特定手段12は、前記式(1−2)及びそのただし書きの式に基づいて、上記Paの逆変換の射影変換行列Pa-1を決定し、このPa-1による射影変換を第2の撮像画像中の領域Aa2内の画像に対して施す。これにより、第1の撮像画像に設定した領域Aa1に対応する第2の撮像画像の領域Aa2に、上段側平面PLaの平面パラメータベクトル↑maに応じた射影変換を施してなる画像が算出される。
上記と同様に、S5では、段差エッジ特定手段12は、平坦面部認識手段11により先に算出された下段側平面PLbの平面パラメータベクトル↑mbを式(1−1)の↑mに代入することで射影変換行列Pbを決定し、このPbによる射影変換を第1の撮像画像の領域Ab1の外形枠に施すことによって、Ab1に対応する第2の撮像画像中の領域Ab2を求める。そして、段差エッジ特定手段12は、上記Pbの逆変換の射影変換行列Pb-1を決定し、このPb-1による射影変換を第2の撮像画像中の領域Ab2内の画像に対して施す。これにより、第1の撮像画像に設定した領域Ab1に対応する第2の撮像画像の領域Ab2に、下段側平面PLbの平面パラメータベクトル↑mbに応じた射影変換を施してなる画像が算出される。
また、S6では、S2の処理により算出した起立平面PLcの平面パラメータベクトル↑mcを式(1−1)の↑mに代入することで射影変換行列Pcを決定し、このPcによる射影変換を第1の撮像画像の領域Ac1の外形枠に施すことによって、Ac1に対応する第2の撮像画像中の領域Ac2を求める。そして、段差エッジ特定手段12は、上記Pcの逆変換の射影変換行列Pc-1を決定し、このPb-1による射影変換を第2の撮像画像中の領域Ac2内の画像に対して施す。これにより、第1の撮像画像に設定した領域Ac1に対応する第2の撮像画像の領域Ac2に、起立平面PLcの平面パラメータベクトル↑mcに応じた射影変換を施してなる画像が算出される。
補足すると、本実施形態では、S4、S5、S6のそれぞれの処理において、第1の撮像画像に設定した各領域Ax1(x=a又はb又はc。以降、同様)に対応する第2の撮像画像中の領域Ax2を、平面パラメータベクトル↑mxに応じて決定した射影変換行例Pxによる射影変換によって算出した上で、第2の撮像画像中の領域Ax2内の画像に、Pxの逆変換の射影変換行列Px-1による射影変換を施すことによって、第1の撮像画像の領域Ax1に対応する第2の撮像画像中の領域Ax2の画像の射影変換画像を求めるようにした。
但し、第1の撮像画像の各領域Ax1に対応する第2の撮像画像中の領域Ax2そのものを求めたり、射影変換行列Px-1による射影変換を第2の撮像画像に対して実際に行なうことを必須とするものではなく、結果的に、第1の撮像画像の領域Ax1に対応する第2の撮像画像中の領域Ax2の画像の射影変換画像を求めることができる処理であれば、上記の手法と異なる手法を採用してもよい。
例えば、図9を参照して、第1の撮像画像中の各領域Ax1内の各画素位置Q1に対応する第2の撮像画像上の画素位置Q2を、平面パラメータベクトル↑mxに応じて決定した射影変換行例Pxによる射影変換によって算出し、第1の撮像画像中の領域Ax1の各画素位置Q1の画素値を、その画素位置に対応して算出した第2の撮像画像上の画素位置Q2における該第2の撮像画像の画素値で置き換える。このようにしても、結果的に、第1の撮像画像の領域Ax1に対応する第2の撮像画像中の領域Ax2の画像の射影変換画像を得ることができる。従って、第1の撮像画像の各領域Ax1に対応する第2の撮像画像中の領域Ax2そのものを求めたり、射影変換行列Px-1による射影変換を第2の撮像画像に対して実際に行なうことを実行せずとも、第1の撮像画像の領域Ax1に対応する第2の撮像画像中の領域Ax2の画像の射影変換画像を得ることができる。
以上が、本実施形態におけるS4、S5、S6の処理である。
ここで、暫定段差エッジパラメータにより規定される暫定段差エッジラインLeが、実際の段差エッジに合致する場合には、第1の撮像画像の領域Aa1,Ab1,Ac1は、それぞれ、その全体もしくは大部分が、上段側平面PLa上の部分の画像、下段側平面PLb上の部分の画像、起立平面PLc上の部分の画像に相当するものとなる。これは、領域Aa1,Ab1,Ac1を前記した如く設定することによって、領域Aa1,Ab1,Ac1は、暫定段差エッジパラメータにより規定される暫定段差エッジラインLeが、実際の段差エッジに合致している場合に、それぞれ、上段側の平坦面部51aの部分の画像、下段側の平坦面部51bの部分の画像、段鼻の先端面の部分の画像となるように設定されるからである(図5(b)を参照)。
このため、暫定段差エッジラインLeが、実際の段差エッジに合致する場合には、第1の撮像画像の領域Aa1,Ab1,Ac1にそれぞれ対応する第2の撮像画像中の領域Aa2,Ab2,Ac2の各画像を射影変換した場合に得られる画像(以降、射影変換画像という)は、それぞれ、第1の撮像画像の領域Aa1,Ab1,Ac1のそれぞれの実際の画像に一致もしくはほぼ一致することとなる。
一方、暫定段差エッジラインLeが、実際の段差エッジと合致していない場合には、第2の撮像画像の領域Aa2,Ab2,Ac2の各画像の射影変換画像は、それぞれ、第1の撮像画像の領域Aa1,Ab1,Ac1の実際の画像に対して差異を生じることとなる。そして、その差異の度合いは、暫定段差エッジラインLeと実際の段差エッジとの一致度合いに応じて変化する。
そこで、本実施形態では、段差エッジ特定手段12は、以下に説明するS7〜S10の処理によって、暫定段差エッジラインLeと実際の段差エッジとの一致度合いに応じて値が変化する所定の評価関数を、第1の撮像画像の領域Aa1,Ab1,Ac1の各画像の画素値データと、第2の撮像画像の領域Aa2,Ab2,Ac2の各画像の射影変換画像の画像値データとから算出する。そして、段差エッジ特定手段12は、この評価関数によって暫定段差エッジラインLeと実際の段差エッジとの一致度合いを評価し、その評価結果に応じて暫定段差エッジパラメータを適宜修正することによって、該評価関数の値を最小化する段差エッジパラメータ(評価関数最小化段差エッジパラメータ)を探索的に特定する。このようにして特定される評価関数最小化段差エッジパラメータが、実際の段差エッジの位置及び方向を表す段差エッジパラメータとして得られる。
上記S7〜S10の処理を具体的に説明すると、段差エッジ特定手段12は、S4〜S6の処理に続いて、S7〜S9の処理を実行する。S7では、段差エッジ特定手段12は、第1の撮像画像の領域Aa1の画像の画素値データと、第2の撮像画像の領域Aa2の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Da1を次式(2)により算出する。
また、S8では、段差エッジ特定手段12は、第1の撮像画像の領域Ab1の画像の画素値データと、第2の撮像画像の領域Ab2の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Db1を次式(3)により算出する。
また、S9では、段差エッジ特定手段12は、第1の撮像画像の領域Ac1の画像の画像値データと、第2の撮像画像の領域Ac2の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Dc1を次式(4)により算出する。
これらの誤差関数Da1,Db1,Dc1はそれぞれ、本発明における第2誤差関数、第3誤差関数、第1誤差関数に相当するものである。
式(2)、(3)、(4)において、I1[u]は、第1の撮像画像の座標位置uでの画素値、I2[w(u ;↑mx)](但し、↑mx=↑ma又は↑mb又は↑mc)は、第2の撮像画像の↑mxに応じた射影変換によってから得られる射影変換画像において、第1の撮像画像の座標位置uに対応する座標位置w(u ;↑mx)での画素値である。また、式(2)におけるΣ、式(3)におけるΣ、式(4)におけるΣは、それぞれ、第1の撮像画像の領域Aa1,Ab1,Ac1における積算演算を意味する。
なお、画素値I1[u],I2[w(u ;↑mx)]は、一般的には、各画素の輝度値でよいが、第1の撮像画像及び第2の撮像画像が、カラー画像である場合には、彩度や色相等を使用することもできる。
また、式(2)のNa1、式(3)のNb1、式(4)のNc1はそれぞれ、領域Aa1,Ab1,Ac1の画素数である。
上記式(2)により算出される誤差関数Da1は、第1の撮像画像の領域Ac1の画像の画素値分布と、第2の撮像画像の領域Ac2の画像の射影変換画像の画素値分布とが一致していれば最小となり、両画素値分布の誤差が大きいほど、誤差関数Da1の値が大きくなる。このことは、誤差関数Db1、Dc1についても同様である。
なお、本実施形態では、誤差関数Da1を、I1[u]とI2[w(u ;↑ma)]との差の2乗値を領域Aa1で積算したものとしたが、誤差関数Da1は、第1の撮像画像の領域Aa1の画像の画素値分布と、第2の撮像画像の領域Aa2の画像の射影変換画像の画素値分布との誤差の程度を表現できるものであれば、その他の形態のものであってもよい。例えば、I1[u]とI2[w(u ;↑ma)]との差の絶対値を領域Aa1で積算したものを誤差関数Da1として使用してもよい。このことは、誤差関数Db1,Dc1についても同様である。
次いで、段差エッジ特定手段12は、S10の処理を実行する。このS10では、段差エッジ特定手段12は、S7〜S9でそれぞれ算出した誤差関数Da1,Db1,Dc1の値を、次式(5)により線形結合することによって、評価関数E1の値を算出する。
E1=Da1+Db1+Dc1 ……式(5)
この場合、第1の撮像画像の領域Aa1,Ab1,Ac1は、暫定段差エッジパラメータに依存して設定される領域であるので、上記評価関数E1も、暫定段差エッジパラメータに依存する(暫定エッジパラメータの関数となる)。そして、前記した如く、暫定段差エッジラインLeが、実際の段差エッジに合致する場合には、第1の撮像画像の領域Aa1,Ab1,Ac1の各画像が、それぞれ、第2の撮像画像の領域Aa2,Ab2,Ac2のそれぞれの画像の射影変換画像に一致もしくはほぼ一致するので、誤差関数Da1,Db1,Dc1の値がそれぞれ最小となる。ひいては、評価関数E1の値が最小となる。
なお、式(5)の線形結合は、単純な加算演算であるが、Da1,Db1,Dc1にそれぞれ重み係数を付して、Da1,Db1,Dc1を線形結合することによって、評価関数E1を算出するようにしてもよい。
上記の如く評価関数E1を算出した後、段差エッジ特定手段12は、次に、S1において、上記評価関数E1の値を最小化する(“0”に近づける)ように、暫定段差エッジパラメータを更新する。
この更新処理は、例えば次のように行われる。なお、ここでの説明では、更新前の最新の暫定段差エッジパラメータを更新前暫定段差エッジパラメータ、更新後の暫定段差エッジパラメータを更新後暫定段差エッジパラメータという。
以下説明すると、段差エッジ特定手段12は、暫定段差エッジラインLeの空間的な位置及び方向の一方又は両方、あるいは、第1の撮像画像における画像上暫定段差エッジラインLepの二次元的な位置及び方向の一方又は両方を所定の微小量だけ、更新前暫定段差エッジパラメータに対応する値から変更し、その変更後のLe又はLepに対応する段差エッジパラメータを、観測用段差エッジパラメータとする。
そして、段差エッジ特定手段12は、前記画像処理領域AR1を、図10に示すように第1の撮像画像の横方向に分割してなる複数の微小領域δARのそれぞれにおいて、上記観測用段差エッジパラメータを更新前暫定段差エッジパラメータの代わりに用いて前記S3〜S10と同様の処理を実行することによって、各微小領域δAR毎の評価関数E1の値E1δを算出する。
そして、段差エッジ特定手段12は、各微小領域δAR毎の評価関数E1の値E1δと、更新前暫定段差エッジパラメータに対応するS10の処理で先に算出した評価関数E1の値との偏差に応じて、各微小領域δAR毎に、画像上暫定段差エッジラインLepの二次元的な位置(第1の撮像画像の縦方向での位置)のずらし量(更新前暫定段差エッジパラメータにより規定される画像上段差エッジパラメータLepからのずらし量)を決定する。そのずらし量は、例えば、上記偏差の符号(極性)に応じて、あるいは、該偏差に所定値のゲインを乗じることで決定される。
次いで、段差エッジ特定手段12は、各微小領域δAR毎に、図10に示す如く、更新前暫定段差エッジパラメータにより規定される画像上暫定段差エッジラインLepとしての更新前Lep上の点(例えば、各微小領域δARでの更新前Lepの中央点)から上記ずらし量だけ第1の撮像画像の縦方向にずらした点Pδを算出する。
次いで、段差エッジ特定手段12は、各微小領域δAR毎の点Pδを近似的に結ぶ直線を、更新後暫定段差エッジパラメータにより規定される暫定段差エッジラインLep(更新後Lep)とし、該更新後Lepを最小二乗法により算出する。そして、段差エッジ特定手段12は、この更新後Lepに対応する暫定段差エッジパラメータを更新後暫定段差エッジパラメータとして決定する。
以上が、S1における暫定段差エッジパラメータの更新処理である。
なお、暫定段差エッジパラメータの更新の仕方は、上記した手法に限られるものでなく、種々様々な公知の探索手法を利用できる。例えば、観測用段差エッジパラメータを複数種類、設定し、それぞれの観測用段差エッジパラメータを暫定段差エッジパラメータの代わりに用いて、S2〜S10と同じ処理を実行することで、各観測用段差エッジパラメータ毎の評価関数E1の値を算出する。そして、それらの評価関数E1の値と、更新前暫定段差エッジパラメータに応じて算出した評価関数E1の値との偏差を基に、最急降下法等の勾配法の手法によって、評価関数E1の値を“0”に近づける更新後暫定段差エッジパラメータを決定するようにしてもよい。
段差エッジ特定手段12は、以上の如く暫定段差エッジパラメータを更新するS1の処理と、それに続くS2〜S10の処理とを繰り返す。そして、段差エッジ特定手段12は、n回目のS10の処理で算出した評価関数E1の値と、n−1回目のS10の処理で算出した評価関数E1の値とを比較し、それらの値の差の絶対値が、“0”近傍の所定の設定値よりも小さくなった場合にn回目のS2〜S10の処理で使用した暫定段差エッジパラメータを、実際の段差エッジ52の位置及び方向を表す最適段差エッジパラメータとして決定する。
これにより、評価関数E1の値を最小化するように最適段差エッジパラメータが探索的に特定されることとなる。例えば、n回目の暫定段差エッジパラメータにより規定される画像上暫定段差エッジラインLepが図5(b)に示すLep(n)である場合、このLep(n)に対応する暫定段差エッジパラメータが最適段差エッジパラメータとして決定される。
なお、n回目のS10の処理で算出した評価関数E1の値が、“0”近傍の所定の設定値よりも小さい場合、あるいは、暫定段差エッジパラメータを更新するS1の処理と、それに続くS2〜S10の処理との繰り返し回数nが、所定回数に達した場合に、n回目の処理で使用した暫定段差エッジパラメータを、実際の段差エッジ52の位置及び方向を表す最適段差エッジパラメータとして決定するようにしてもよい。
以上が、カメラ視線が下・上方向の場合における段差エッジ特定手段12の処理である。
次に、カメラ視線が上・下方向の場合における段差エッジ特定手段12の処理を説明する。この説明においては、段差部が階段50であって、カメラ3Rの撮像画像(第1の撮像画像)が、図6(a),(b)に示したような画像となる場合を主要例として段差エッジ特定手段12の処理を説明する。この場合、図6(a),(b)に示す平坦面部投影領域Ar3,Ar4にそれぞれ対応する階段50の平坦面部は、それぞれ、図4(a),(b)に示す上段側の平坦面部51a、その一段下側の下段側の平坦面部51bであるとする。
カメラ視線が上・下方向の場合には、図6(a),(b)に例示した第1の撮像画像のように、カメラ3R,3Lの各撮像画像には、階段50の段鼻の先端面に相当する面(段差エッジから垂下する面)が写らない。
このため、この場合には、段差エッジ特定手段12は、図7のブロック図に示した処理から、起立平面PLcに関連する処理を省略してなる図11のブロック図に示す処理によって、実際の段差エッジの位置及び方向の推定値を表す最適段差エッジパラメータを探索的に特定する。
以下説明すると、段差エッジ特定手段12は、まず、S21の処理を実行し、暫定段差エッジパラメータの初期値を設定する。この暫定段差エッジパラメータの初期値の設定の仕方は、前記S1の処理と同じでよい。この場合、画像上暫定段差エッジラインLepが、例えば、図6(a)に示したLep(0)となるような暫定段差エッジパラメータの初期値が設定される。但し、カメラ視線が上・下方向の場合では、暫定段差エッジパラメータの初期値により規定される画像上暫定段差エッジラインLep(0)が、上段側の平坦面部51aに対応する平坦面部投影領域Ar3から逸脱するように設定されていてもよい。
次いで、段差エッジ特定手段12は、S22の処理を実行し、第1の撮像画像中の画像処理領域AR2内に、射影変換(平面射影変換)に用いる2つの領域Aa3,Ab3を、S21で設定した暫定段差エッジパラメータに応じて設定する。
ここで、S22の処理で使用する画像処理領域AR2は、図6(a),(b)に破線で示すように設定される方形状の枠内領域である。この画像処理領域AR2は、その下部が、階段50の上段側の平坦面部51aに対応する平坦面部投影領域Ar1に存在し、且つ、画像処理領域ARの上部が、階段50の下段側の平坦面部51bの平坦面部投影領域Ar2に存在するように第1の撮像画像中に設定される。また、この画像処理領域AR2の横方向の幅は、例えば、概ねロボット1の通過領域と同程度の幅となるように設定される。
そして、この画像処理領域AR2内に設定する2つの領域Aa3,Ab3はそれぞれ、本発明における第4領域、第5の領域に相当するものであり、これらは、次のように設定される。
すなわち、第1の撮像画像において、画像上暫定段差エッジラインLepが領域Aa3,Ab3の境界線となると共に、領域Aa3が、上段側の平坦面部51aと下段側の平坦面部51bとのうちの上段側の平坦面部51a寄りの領域、領域Ab3が下段側の平坦面部51b寄りの領域となるように、これらの領域Aa3,Ab3が設定される。
図6(a),(b)に示す第1の撮像画像においては、画像処理領域AR2のうちの、画像上暫定段差エッジラインLep(図6(a)ではLep(0)、図6(b)ではLep(n))の下側の領域が領域Aa3、上側の領域が領域Ab3として設定される。
なお、前記S22の処理は、本発明における評価関数算出用領域設定手段を実現するものである。
次いで、段差エッジ特定手段12は、S23、S24の処理を実行する。S23では、段差エッジ特定手段12は、図7のS4の処理と同様に、上段側平面PLaの平面パラメータベクトル↑maを前記式(1−1)の↑mに代入することで決定した射影変換行列Paにより、第1の撮像画像の領域Aa3の外形枠に射影変換を施すことによって、Aa3に対応する第2の撮像画像中の領域(以下、これを領域Aa4という)を決定する。そして、段差エッジ特定手段12は、Paの逆変換の射影変換行列Pa-1による射影変換を第2の撮像画像中の領域Aa4の画像に施すことによって、領域Aa4の射影変換画像を求める。
また、S24では、段差エッジ特定手段12は、図7のS5の処理と同様に、下段側平面PLbの平面パラメータベクトル↑mbを式(1−1)の↑mに代入することで決定した射影変換行列Pbにより、第1の撮像画像の領域Ab3の外形枠に射影変換を施すことによって、該領域Ab3に対応する第2の撮像画像中の領域(以下、これを領域Ab4という)を決定する。そして、段差エッジ特定手段12は、Pbの逆変換の射影変換行列Pb-1による射影変換を第2の撮像画像中の領域Ab4の画像に施すことによって、領域Ab4の射影変換画像を求める。
このようにして算出される第2の撮像画像の領域Aa4,Ab4の各画像の射影変換画像は、暫定段差エッジラインLeが、実際の段差エッジに合致する場合には、それぞれ、第1の撮像画像の領域Aa3,Ab3のそれぞれの実際の画像に一致もしくはほぼ一致することとなる。
そして、暫定段差エッジラインLeが、実際の段差エッジと合致していない場合には、第2の撮像画像の領域Aa4,Ab4の各画像の射影変換画像は、それぞれ、第1の撮像画像の領域Aa3,Ab3の実際の画像に対して差異を生じ、その差異の度合いが、暫定段差エッジラインLeと実際の段差エッジとの一致度合いに応じて変化する。
そこで、段差エッジ特定手段12は、次に、図7のS7〜10と同様の処理をS25〜S27で実行し、暫定段差エッジラインLeと実際の段差エッジとの一致度合いに応じて値が変化する評価関数を算出する。
具体的には、S25では、段差エッジ特定手段12は、第1の撮像画像の領域Aa3の画像の画像値データと、第2の撮像画像の領域Aa4の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Da2を次式(6)により算出する。
また、S26では、段差エッジ特定手段12は、第1の撮像画像の領域Ab3の画像の画像値データと、第2の撮像画像の領域Ab4の画像の射影変換画像の画素値データとから、それらの画素値分布の誤差の程度を表す誤差関数Db2を次式(7)により算出する。
これらの誤差関数Da2,Db2はそれぞれ、本発明における第4誤差関数、第5誤差関数に相当するものである。
式(6)、(7)において、I1[u]、I2[w(u ;↑mx)](但し、↑mx=↑ma又は↑mb)の意味は、前記式(2)〜(4)のものと同じである。また、式(6)におけるΣ、式(7)におけるΣは、それぞれ、第1の撮像画像の領域Aa3,Ab3における積算演算を意味する。また、式(6)のNa2、式(7)のNb2はそれぞれ、領域Aa3,Ab3の画素数である。
なお、誤差関数Da2,Db2は、前記式(2)〜(4)の誤差関数Da1,Db1,Dc1に関して説明した場合と同様に、その他の形態のものであってもよい。例えば、I1[u]とI2[w(u ;↑ma)]との差の絶対値を領域Aa3で積算したものを誤差関数Da2として使用してもよい。
次いで、段差エッジ特定手段12は、S27の処理を実行する。このS27では、段差エッジ特定手段12は、S25,26でそれぞれ算出した誤差関数Da2,Db2の値を、次式(8)により線形結合することによって、評価関数E2の値を算出する。
E2=Da2+Db2 ……式(8)
この場合、第1の撮像画像の領域Aa3,Ab3が前記したように設定されるので、上記のように算出される評価関数E2は、その値が、暫定段差エッジパラメータに依存するものとなる。そして、暫定段差エッジラインLeが、実際の段差エッジに合致する場合には、誤差関数Da2,Db2の値がそれぞれ最小(≒0)となるので、評価関数E2の値も最小となる。
なお、Da2,Db2に重み係数を付して、Da2,Da1を線形結合することによって評価関数E2を算出するようにしてもよい。
上記の如く、評価関数E2を算出した後、段差エッジ特定手段12は、次に、S21において、上記評価関数E2の値を最小化する(“0”に近づける)ように、暫定段差エッジパラメータを更新する。そして、段差エッジ特定手段12は、暫定段差エッジパラメータを更新するS21の処理と、それに続くS22〜S27の処理とを繰り返す。
この場合、S21における暫定段差エッジパラメータの更新は、図7のS1での更新処理と同じ手法で行なわれる。
そして、段差エッジ特定手段12は、n回目のS27の処理で算出した評価関数E2の値と、n−1回目のS27の処理で算出した評価関数E2の値とを比較し、それらの値の差の絶対値が、“0”近傍の所定の設定値よりも小さくなった場合にn回目のS22〜S27の処理で使用した暫定段差エッジパラメータを、実際の段差エッジ52の位置及び方向を表す最適段差エッジパラメータとして決定する。
これにより、評価関数E2の値を最小化するように最適段差エッジパラメータが探索的に特定されることとなる。例えば、n回目の暫定段差エッジパラメータにより規定される画像上暫定段差エッジラインLepが図6(b)に示すLep(n)である場合、このLep(n)に対応する暫定段差エッジパラメータが最適段差エッジパラメータとして決定される。
なお、S21での暫定段差エッジパラメータの更新処理では、S1での更新処理に関して補足した如く、種々様々な公知の探索手法を利用できる。また、n回目のS27の処理で算出した評価関数E2の値が、“0”近傍の所定の設定値よりも小さい場合、あるいは、暫定段差エッジパラメータを更新するS21の処理と、それに続くS22〜S27の処理との繰り返し回数nが、所定回数に達した場合に、n回目の処理で使用した暫定段差エッジパラメータを、実際の段差エッジ52の位置及び方向の推定値を表す最適段差エッジパラメータとして決定するようにしてもよい。
以上が、カメラ視線が上・下方向の場合における段差エッジ特定手段12の処理である。
以上説明した本実施形態によれば、カメラ視線が下・上方向である場合と、上・下方向である場合とのいずれの場合でも、第1の撮像画像と第2の撮像画像とから構成されるステレオ画像と、射影変換(平面射影変換)とを利用した探索手法によって、実際の段差エッジ52の位置及び方向の推定値を表す最適段差エッジパラメータを特定できる。
この場合、カメラ視線が下・上方向である場合には、段差エッジ特定手段12は、暫定段差エッジパラメータに応じて第1の撮像画像の画像処理領域AR1内に領域Aa1,Ab1,Ac1を設定し、それぞれの領域Aa1,Ab1,Ac1の画像の画素値分布と、それぞれの領域Aa1,Ab1,Ac1に対応する平面パラメータ(平面パラメータベクトル↑ma,↑mb,↑mc)に応じた射影変換を第2の撮像画像中の対応領域Aa2,Ab2,Ac2の画像に施してなる射影変換画像の画素値分布と誤差、ひいては、前記評価関数E1の値を最小化するように暫定段差エッジパラメータを更新していくことで、最適段差エッジパラメータを探索的に特定する。
また、カメラ視線が上・下方向である場合には、段差エッジ特定手段12は、暫定段差エッジパラメータに応じて第1の撮像画像の画像処理領域AR2内に領域Aa3,Ab3を設定し、それぞれの領域Aa3,Ab3の画像の画素値分布と、それぞれの領域Aa3,Ab3に対応する平面パラメータ(平面パラメータベクトル↑ma,↑mb)に応じた射影変換を第2の撮像画像中の対応領域Aa4,Ab4の画像に施してなる射影変換画像の画素値分布と誤差、ひいては、前記評価関数E2の値を最小化するように暫定段差エッジパラメータを更新していくことで、最適段差エッジパラメータを探索的に特定する。
これにより、カメラ視線が下・上方向である場合及び上・下方向である場合のいずれの場合においても、各撮像画像における段差エッジそのものが、その周辺の部分の画像との区別がつきにくいものとなっている場合であっても、最適段差エッジパラメータを高いロバスト性で安定に特定することができる。
特に、カメラ視線が下・上方向である場合には、領域Ac1の上側の境界線としての画像上段差エッジラインLepとこれと反対側の境界線(下側の境界線)との間の間隔Δc、並びに、領域Ac1の下側の境界線と領域Ab1の上側の境界線との間の間隔Δbとを前記した如く設定したことによって、階段50の上段側の平坦面部51aと、下段側の平坦面部51bとの間の蹴込みに相当する部分の画像が評価関数E1の値に影響を及ぼすのを抑制することができる。
このため、上段側の平坦面部51aと下段側の平坦面部51bとの間に、上段側平面PLa、起立平面PLc、及び下段側平面PLbから逸脱した部分が存在する場合であっても、実際の段差エッジの位置及び方向を表すものとして信頼性の高い最適段差エッジパラメータを、高いロバスト性で安定に特定することができる。
次に、本発明の他の態様の実施形態をいくつか説明しておく。
前記実施形態では、カメラ視線が下・上方向である場合における図7のS3の処理で、第1の撮像画像での領域Ab1の上側の境界線を、領域Ac1の下側の境界線と所定の間隔Δbを存して平行に延在するように設定したが、領域Ab1の上側の境界線を次のように設定してもよい。すなわち、図12に示すように、起立平面PLc(図7のS2で設定した起立平面パラメータにより規定される起立平面)と、下段側平面PLbとの交線を第1の撮像画像に投影してなる直線上の線分(当該直線のうちの画像処理領域AR1内の部分)を、領域Ab1の上側の境界線として設定してもよい。
このようにすることで、前記の実施形態と同様に、階段50の上段側の平坦面部51aと、下段側の平坦面部51bとの間の蹴込みに相当する部分の画像が評価関数E1の値に影響を及ぼすのを抑制することができる。また、撮像カメラ3R,3Lと階段50(段差部)との距離によらずに、暫定段差エッジラインLeが、実際の段差エッジに合致もしくはほぼ合致する場合に、領域Ab1の上側の境界線が、下段側の平坦面部51bに存在するようにすることができる。従って、暫定段差エッジラインLeと、実際の段差エッジとの一致度合いを、前記誤差関数Db1に適切に反映させ、ひいては、前記評価関数E1の信頼性を適切に確保することができる。
また、カメラ視線が上・下方向である場合においては、第1の撮像画像における下段側の平坦面部51bの画像のうち、位置及び方向を推定しようとする実際の段差エッジに近接した部分の画像が、第2の撮像画像中には写っていない(上段側の平坦面部51aに陰に隠れてしまう)というようなオクルージョンが発生する場合がある。
そして、このような場合に、前記実施形態の如く、第1の撮像画像おける画像上暫定段差エッジラインLepが領域Aa3,Ab3の境界線となるように領域Aa3,Ab3を設定すると、暫定段差エッジラインLeが、実際の段差エッジに合致もしくはほぼ合致する状況で、領域Ab3を下段側平面PLbの平面パラメータベクトル↑mbに応じて射影変換してなる第2の撮像画像中の領域(領域Ab3に対応する第2の撮像画像中の領域)に、上段側の平坦面部51aの画像が含まれてしまうこととなる。ひいては、前記式(7)により算出される誤差関数Db2に、上記の如きオクルージョンに起因する誤差成分がノイズ成分として含まれることとなる。
従って、上記の如きオクルージョンが発生する状況では、暫定段差エッジラインLeが、実際の段差エッジに合致もしくはほぼ合致する場合に、該オクルージョンに起因する誤差成分が、前記式(7)により算出される誤差関数Db2に含まれるのを防止するように、前記領域Ab3を設定することが望ましい。
カメラ視線が上・下方向である場合における図11のS22の処理において、このようにオクルージョンの発生を考慮して領域Aa3,Ab3を設定する手法の一例を以下に図13を参照して説明する。なお、図13では、便宜上、第1の撮像画像における画像上暫定段差エッジラインLep(k)は、実際の段差エッジの画像に概ね合致しているものとして示している。
この手法では、段差エッジ特定手段12は、領域Aa3を、前記の実施形態と同様に設定する。すなわち、画像上暫定段差エッジラインLep(k)が、領域Aa3の上側の境界線とされ、第1の撮像画像内の画像処理領域AR2のうち、画像上暫定段差エッジラインLep(k)から下側の領域が、領域Aa3として設定される。
一方、段差エッジ特定手段12は、領域Ab3を次のように設定する。まず、段差エッジ特定手段12は、上段側平面PLaの平面パラメータベクトル↑maを前記式(1−1)の↑mに代入することで決定した射影変換行列Paにより、第1の撮像画像における画像上暫定段差エッジラインLep(k)(kは、暫定段差エッジパラメータの更新回数を表す)を射影変換する。これにより、図13に示される如く、第2の撮像画像上の線分L1が得られる。
また、段差エッジ特定手段12は、下段側平面PLbの平面パラメータベクトル↑mbを前記式(1−1)の↑mに代入することで決定した射影変換行列Pbにより、第1の撮像画像における画像上暫定段差エッジラインLep(k)を射影変換する。これにより、図13に示される如く、第2の撮像画像上の線分L2が得られる。
ここで、オクルージョンが発生している場合には、第2の撮像画像において、線分L1は、線分L2よりも上段側の平坦面部51aから下段側の平坦面部51bに向う向き(概ね上向き)に変位した位置に存在する。
そこで、段差エッジ特定手段12は、第2の撮像画像において、線分L1が、線分L2よりも上段側の平坦面部51aから下段側の平坦面部51bに向う向き(概ね上向き)に変位した位置に存在するか否かを判断する(以降、この判断処理をオクルージョン発生判断処理という)。そして、このオクルージョン発生判断処理の判断結果が肯定的である場合には、段差エッジ特定手段12は、領域Ab3の下側の境界線L3を、図13に示す如く画像上暫定段差エッジラインLep(k)(領域Aa3の上側の境界線)よりも下段側の平坦面部51b寄りの位置で、Lep(k)と間隔を存してほぼ平行に延在するように設定する。そして、段差エッジ特定手段12は、画像処理領域AR2のうち、上記境界線L3から上側の領域を領域Ab3として設定する。
この場合、上記境界線L3は、より具体的には次のように設定される。すなわち、第1の撮像画像における画像上暫定段差エッジラインLep(k)を射影変換行列Paにより射影変換した場合に得られた線分L1を、下段側平面パラメータ↑mbに応じた射影変換行列Pbの逆変換の射影変換行列Pb-1によって射影変換することで得られる線が、領域Ab3の下側の境界線L3として設定される。
また、オクルージョン発生判断処理の判断結果が否定的である場合には、段差エッジ特定手段12は、画像上暫定段差エッジラインLep(k)(領域Aa3の上側の境界線)を、領域Ab3の下側の境界線L3とし、画像処理領域AR2のうち、該境界線L3から上側の領域を領域Ab3として設定する。
以上が、オクルージョンが発生している場合の領域Aa3,Ab3の設定手法の一例である。
カメラ視線が上・下方向である場合における図11のS22の処理において、上記の如く領域Aa3,Ab3を設定することによって、オクルージョンに起因する誤差成分が、前記式(7)により算出される誤差関数Db2に含まれるのを防止することができる。このため、前記式(8)により算出される評価関数E2が、暫定段差エッジLeと、実際の段差エッジとの一致度合いを表すものとしての信頼性をより一層高めることができる。その結果、実際の段差エッジの位置及び方向を表すものとして最終的に特定される最適段差エッジパラメータの信頼性をより一層高めることができる。
補足すると、以上説明した領域Aa3,Ab3の設定手法の例では、第1の撮像画像における画像上暫定段差エッジラインLep(k)が、本発明における第1暫定段差エッジ線に相当し、第2の撮像画像上の線分L1,L2がそれぞれ、本発明における第2A暫定段差エッジ線、第2B暫定段差エッジ線に相当する。
また、前記実施形態では、暫定段差エッジパラメータを、前記評価関数E1又はE2の値に応じて初期値から逐次更新していく処理によって、最適段差エッジパラメータを特定するようにした。但し、最適段差エッジパラメータの特定を以下に説明するように行なうようにしてもよい。
例えばカメラ視線が下・上方向である場合について代表的に説明すると、前記段差エッジ特定手段12により最適段差エッジパラメータを特定する場合に、まず、実際の段差エッジの位置及び方向の推定値を表すものとして採用し得る可能性のある暫定段差エッジパラメータの複数の候補を決定しておく。
この複数の暫定段差エッジパラメータの候補は、例えば次のように決定される。すなわち、各候補に対応する画像上暫定段差エッジラインLepが、第1の撮像画像内で前記平坦面部認識手段11によって抽出された上段側の平坦面部51aに対応する平坦面部投影領域Ar1の下側の境界線(下段側の平坦面部51bに対応する平坦面部投影領域Ar2寄りの境界線)の近辺に存在し、且つ、それらの各候補に対応する画像上暫定段差エッジラインLepの位置(具体的には、第1の撮像画像の画像処理領域AR1内での画像上暫定段差エッジラインLepの中央点の位置(第1の撮像画像の縦方向での位置)と、方向(具体的には、第1の撮像画像の横方向に対する傾斜角度)との両方又は一方が各候補毎に互いに異なるものとなるように決定される。
この場合、各候補に対応する画像上暫定段差エッジラインLepの位置の種類数と該画像上暫定段差エッジラインLepの方向の種類数とは、それぞれ、所定数m1、m2(m1,m2は2以上の整数)とされ、トータルm1×m2個の暫定段差エッジパラメータの候補が決定される。なお、各候補に対応する画像上暫定段差エッジラインLepのm1個の位置は、例えば、上段側の平坦面部51aに対応する平坦面部投影領域Ar1の下側の境界線の近辺の所定範囲内で第1の撮像画像の縦方向に微小量づつ、異なる位置となるように決定される。また、各候補に対応する画像上暫定段差エッジラインLepのm2個の方向は、例えば、第1の撮像画像の横方向に対する傾斜角度(鋭角側の傾斜角度)が0°近辺の所定範囲内で微小量づつ、異なる角度となるように決定される。
そして、上記のように決定した暫定段差エッジパラメータの各候補毎に、前記した図7のS2〜S10と同じ処理を実行することによって評価関数E1の値を算出し、暫定段差エッジパラメータの候補のうち、算出した評価関数E1の値が最小となる候補を、最適段差エッジパラメータとして特定する。
カメラ視線が上・下方向である場合においても、上記と同様に、暫定段差エッジパラメータの複数の候補の中から、最適段差エッジパラメータを特定することができる。この場合は、暫定段差エッジパラメータの複数の候補の設定の仕方は、カメラ視線が下・上方向である場合と同じよい。そして、各候補毎の評価関数E2は、前記した図11のS22〜S27と同じ処理を実行することによって算出するようにすればよい。
また、以上説明した実施形態では、カメラ視線が下・上方向である場合に、評価関数E1を、3つの誤差関数Da1,Db1,Dc1を線形結合することによって構成したが、誤差関数Db1を省略し、誤差関数Da1,Dc1の2つの誤差関数を線形結合することによって評価関数E1を構成するようにしてもよい。この場合には、第1の撮像画像における領域Ab1と、これに対応する第2の撮像画像の領域Ab2の設定は不要である。
また、前記実施形態では、カメラ視線が下・上方向である場合に、上段側平面PLaに対する前記起立平面PLcの傾斜角度を既定値とすることで、段差エッジ特定手段12の演算処理負負荷が軽減されることとなるが、段差エッジパラメータに加えて、該傾斜角度も暫定的に仮決定するようにして、前記評価関数E1の値を最小化するように、段差エッジパラメータと該傾斜角度との両方を探索的に決定するようにしてもよい。
また、前記実施形態では、カメラ3Rの撮像画像を第1の撮像画像として使用する場合を例にとって説明したが、カメラ3Lの撮像画像を第1の撮像画像として使用してもよいことはもちろんである。
また、前記実施形態では、段差部としての階段50の段差エッジの位置及び方向を推定する場合を主要例として説明したが、階段50以外の段差部、例えば、床に設置される踏み台等の設置物により構成される段差部の段差エッジであっても、その位置及び方向を前記実施形態と同様に推定することができる。
また、前記実施形態では、カメラ3R,3Lをロボット1に搭載した場合を例にとって説明したが、カメラ3R,3Lは、階段等の段差部を移動可能なロボット1以外の移動体に搭載したものでもあってもよい。